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Abstract. Humans can effortlessly manipulate objects in their hands, dex- 
terously sliding and twisting them within their grasp. Robots, however, have 
none of these capabilities, they simply grasp objects rigidly in their end effec- 
tors. To investigate this common form of human manipulation, an analysis of 
controlled slipping of a grasped object within a robot hand was performed. 
The Salisbury robot hand demonstrated many of these controlled slipping 
techniques, illustrating many results of this analysis. 

First, the possible slipping motions were found as a function of the lo- 
cation, orientation, and types of contact between the hand and the object. 
Second, for a given grasp, the contact types were determined as a function of 
the grasping force and the external forces on the object. Finally, by chang- 
ing the grasping force, the robot modified the constraints on the object and 
affect controlled slipping motions. 
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Chapter 1 
Introduction 

1.1 Introduction 

When we manipulate objects, our fingers are not always fixed to the surface. 
Many times we allow the objects we hold to slide or rotate at our fingertips, 
consciously controlling the motion of the object rather than the motion of 
our fingers. This controlled slipping technique of manipulation is not just one 
of the ways we can move objects, but rather a dominant form of dexterous 
human manipulation. For example, try putting a lid on a jar, but start with 
the lid top down on a table. Without thinking, we pick up the lid, spin it 
around between our two fingers, using the edge of the jar or another finger, 
and screw it on the top. Or consider the use of a pencil eraser. When we 
make a mistake we stop writing, flip the pencil over, push the pencil through 
our fingers, and erase. In both these examples and in many others, we allow 
objects to slide and rotate at our fingertips. Through this controlled slipping 
technique we can control the location and orientation of an object within our 
grasp. 

In robotic manipulation, emphasis has been placed on producing stable 
grasps. The object is then moved by controlling the motion of the manipula- 
tor, assuming the object is rigidly fixed to the robot. If the object slips in the 
grasp, control is lost, and there are no easy forms of recovery. In addition, 
there are many operations which become difficult or impossible without some 
form of controlled slip manipulation. In this thesis, I will analyze controlled 
slip manipulation in a robot hand, and from this analysis predict slipping 
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motion of an object within a grasp. Using these predicted slipping motions, 
the robot hand will affect the necessary changes in the grasp to allow the 
object to slip in a controlled manner. 

1.2 Overview 

In order for a multifingered robot hand to perform dexterous operations 
on the environment, it must be able to acquire objects into a grasp, con- 
trol the motion of the object relative to the environment and control the 
object within the grasp. Object acquisition is an active area of robotic re- 
search. Given a particular manipulator and an object, how can the robot 
grasp the object? There are numerous considerations, such as the location, 
size, weight, orientation, surface properties, and specific functions of the ob- 
ject, as well as, configuration, workspace, strength, and surface properties of 
the manipulator. An algorithm recently developed [Nguyuan], computes the 
grasp locations on an object and finger stiffnesses in the robot necessary to 
produce force closure grasps. The objects are modeled as polyhedrons with 
a certain mass and surface friction. Areas on the polyhedral surfaces are 
then found on which the fingertips of a multifingered hand may be placed 
to yield force closure grasps. The number of fingers, the surface properties 
of the fingertips, the stiffness of the joints, and the workspace of the hand 
are all taken into account when computing grasp locations. Some research in 
object acquisition has also been done by [Lozano-Perez]. Not only were the 
current constraints on the object considered, but also constraints imposed on 
the object by the environment, as the object is moved through its planned 
trajectory. The constraints both present and future are mapped onto the 
object surface before the object is grasped. In this way, for example, a robot 
gripper would not grab the end of the peg which later must be inserted into 
a hole. There are, however, still many areas of object acquisition not yet 
explored. Objects with special surfaces, such as handles or loops, non-rigid 
objects, like paper or foam, objects that must be moved before grasping, such 
as a coin on a flat table, are all common examples acquisition which are not 
yet possible for a robot. 

The control of object motion through the coordinated control of individ- 
ual manipulators is also a current area of robot research. An algorithm has 
recently been written [Chiu] which coordinates individual fingers to yield a 
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specific translations and rotations of a grasp relative to an arbitrary refer- 
ence frame. The object is assumed to remain fixed relative to the grasp; 
therefore, specific motions of the grasp yield specific motions of the object. 
From practical experience, this method has worked quite well. In order to 
actually control the motion of the object, however, it would be necessary to 
sense the position and orientation of the object, since the object may have 
been removed, slipped, or fallen from the grasp, unknown to the robot. Real 
time hand-eye coordination, however, is beyond the reach of current systems 
because of the computational complexity. High level coordinated control of 
multifingered robot hands, is a relatively new area of research. Recent mul- 
tifingered robot hand research, therefore, has been primarily concerned with 
mechanical design, actuation, transmission, and sensing. 

Control of an object relative to a grasp is also a new area of research. 
This problem has be approached by [Tournassoud], in terms of regrasping. 
The object is initially grasped, then set down, released, and then regrasped. 
In this way the object can be reoriented relative to the grasped. The object, 
however, is not manipulated within the grasp. Controlled slip manipulation, 
however, could enhance the dexterity of a manipulator, by allowing the robot 
greater freedom to move the object within the grasp. 

1.3 Outline 

This thesis is basically an analysis of the small finite permissible motions 
an object may undergo in a particular grasp and how these motions may 
be achieved. The first chapter is the introduction. In chapter 2, the types 
of contacts between a fingertip and an object are enumerated. For each 
set of contacts and contact types, we define a constraint state and for each 
constraint state, a map of the possible object motions is produced. Deter- 
mining the constraint on small finite motion, requires knowing not only is 
the location and orientation of contacts, but also the surface geometry in the 
neighborhood of the contact. Chapter 3 deals with strategies available to ma- 
nipulate an object relative to a grasp. These include the use of gravity, body 
forces, controlled accelerations, free fingers, and other objects to move objects 
within the grasp. For a multifingered hand there are many ways to squeeze a 
grasped object. For two or three fingered grasps, there is corresponding one 
space or three space of possible grasp solutions. Chapter 4 presents a sim- 
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pie intuitive parameterization of this grasp force space. For a three fingered 
hand, the three space may be represented simply by a grasp force focus and a 
grasp force magnitude. In order to determine the type of contact that exists 
at each fingertip, the forces and moments transmitted through the interface 
must be found. In chapter 5, the set of forces and moments that exist at the 
contacts will be found in terms of the contact frame, that is, a coordinate 
frame set in the object and defined in terms of the contact point and outward 
pointing normal. Chapter 6 outlines a simple relationship between the forces 
and moments at the contact and the contact type. Two examples are given 
in chapter 7 illustrating the analyses of the previous chapters, along with an 
demonstration of how controlled slipping could be used to reorient an object 
within a grasp. Chapter 8 describes how these analyses were simulated and 
how some controlled slip manipulations were achieved on the Salisbury robot 
hand. In chapter 9, extensions of the present theories are outlined, which 
yield more practical and efficient techniques for controlled slip manipulation. 



Chapter 2 
Constraint 

2.1 Introduction 

The purpose of this chapter is to determine the different ways an object can 
move in a grasp. First, the screw system representation will be described. 
This representation allows a complete and homogeneous treatment of both 
forces and moments as well as translation and angular displacements. Sec- 
ond, the types of contacts which may occur between two objects will be 
enumerated. For each contact type, a particular set of forces and moments 
can be exerted through the interface. These forces and moments limit the 
possible motions of one object relative to another. Third, the set of infinites- 
imal motions possible for an object subject to a single constraint will be 
determined using the concept of virtual work. Fourth, the set of infinitesi- 
mal motions will be extended to include the set of small finite motions by 
considering the surface geometry in the neighborhood of the contact point. 
Fifth, a constraint state will be defined as an ordered list of contact types on 
the object. Finally, for each constraint state, the set of permissible motions 
for a grasped object will be found by intersecting the permissible motions of 
the individual contacts. 

2.2 Screws, wrenches, and twists 

Many of the analyses in this thesis will employ a screw system representa- 
tion for forces and moments, and for infinitesimal displacements. Although 
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standard force and displacement vectors could be used, screw systems allow 
a homogeneous treatment of both forces and moments, and translational and 
angular displacements. 

A set of forces and moments acting upon a body can be collectively called 
a wrench. A wrench may be described as a force along a unique line, the 
screw axis, and a moment about that line. Similarly, a twist can represent 
the infinitesimal motion of an object, an infinitesimal translation along a line 
and an infinitesimal rotation about that line. 

2.2.1 Screws 

The outline of screw systems presented here is more adequately described in 
[Hunt] and completely developed in [Ball]. Both the wrench and the twist 
are specific representations of a screw. A screw is defined by a line in three 
space, a screw axis, and an associated pitch about that line. A screw may 
also be described by a six element vector, s = [s u s 2 ,s 3 ,s 4 ,s 5 , s 6 ], where 
si,s 2 ,...,s e are the screw coordinates. The coordinates of the screw axis 
are 

L = Si 

M = S 2 

N = S 3 

P = S 4 -pSi (2.1) 

Q = 5 5 - pS 2 

R = Se — PS3. 

The coordinates L,M,...,R are known as the Pliicker line coordinates of 
the axis, where L,M, and TV are proportional to the direction cosines of the 
screw axis, and P,Q, and R are proportional to the moment of the line about 
the origin of the reference frame. The pitch of the screw is 

_ S1S4 + 6*255 + 535*6 

P ~ st + si + st ' (2,2) 

and the magnitude of the screw is 

m = \fsJTsJTsl, (2.3) 
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unless the pitch happens to be infinite, in which case the magnitude of the 

screw is 

m = yJS 2 4 + SI + SI (2.4) 

2.2.2 Wrenches 

The wrench is one interpretation of the screw and, therefore, may be defined 
in a similar way. The wrench may be identified, in terms of screw coordinates, 
by a six element vector w = [wi,w 2 , w 3 , w 4 , w 5 , w 6 ], where Wi,w 2 , and w 3 are 
the forces along the x,y, and z axes of an reference frame, and w 4 ,w 5 , and w 6 
are the moments about the axes of the reference frame. By replacing Si with 
Wi in equation 2.1, the line coordinates of the wrench may be found, 



L = 


Wi 


M = 


w 2 


N = 


w 3 


P = 


w 4 — pwi 


Q = 


w 5 — pw 2 


R = 


w 6 - pw 3 



(2.5) 



The pitch of the wrench is given by equation 2.2, 

_ W!W 4 + w 2 w 5 + w 3 w 6 . 

P w\ + w\ + w 2 3 ' {Z - b) 

and is the ratio of the torque about the screw axis to force along it. The 
magnitude of the wrench from equations 2.3 or 2.4 is 

m = ^JwJTwY+wJ, (2.7) 

or if the pitch is infinite, the magnitude is 

m = \Jw\ + w% + w%. (2.8) 

2.2.3 Twists 

As with the wrench, the twist can also be described in terms of a six element 
vector t = [<x, t 2 , t 3 , t 4 , t 6 , t 6 ], where ti,t 2 , and t 3 are the rotations about the 
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x, y, and z axes of a reference frame and t 4 ,t 5 , and t 6 are the displacements 
along the x,y, and z axes. Again, replacing Si with <,-, the line coordinates 
of the twist are 

L = ti 

M = t 2 

N = t 3 

P = U-pt\ (2.9) 

Q = h- Ph 

R = t 6 — pt 3 . 

The pitch of the twist is 

p tl + tl + tl ' (2 - 10) 

that is, the ratio of the rotation about the twist axis to the translational 
along the axis. The magnitude is defined by 



m = y/tl + % + tl (2.11) 

or in the case where the pitch is infinite, the magnitude is 

™ = y/t\ + % + t%. (2.12) 

2.2.4 Transforming screw definitions 

Although a wrench and a twist are independent of the reference frames used 
to define them, it is useful to be able to transform their representations 
from one frame to another. In general, a screw s' can be represented in 
screw coordinates, s' = [aj, s' 2 , s' 3 , s' 4 , s' 5 , s' 6 ], where the elements of s' are 
defined relative to a specific reference frame. Let the elements of the screw 
s' be defined relative to a specific coordinate frame O'X'Y'Z'. Suppose this 
reference frame is, in turn, defined with respect to another reference frame 
OXYZ, figure 2.1. That is, let 1 = [/„/„,/,], m = [m x ,m y ,m z ], and n = 
[n x ,n y ,n z ], be the unit direction vectors of the x', y', and z' axes and let 
x = [x,y,z] be the origin of the O'X'Y'Z' frame all defined with respect 
to the OXYZ frame. Suppose we now wish to define the screw s' in screw 
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coordinates relative to the OXYZ frame. Let s be the representation of the 
screw in the OXYZ frame, then 



s = Ts', 
where T is a linear transformation matrix given by 



T = 



(2.13) 



/* 


m x 


n 


h 


m y 


n 


I, 


m z 


n 



"V 

l z y - l y z m z y - rriyZ n z y - n y z 

l x z — l z x m x z — m z x n x z — n z x 

l y x - l x y m y x - m x y n y x - n x y 



l* 


m x 


n x 


ly 


m y 


n y 


h 


m z 


n z 



(2.14) 



Conversely, a screw s defined in the OXYZ frame may also be represented 
in O'X'Y'Z', 

a' = T~\ (2.15) 

where T _1 equals 



T -i = 



L L 



L 



m, 
n. 



z 

m z 
n, 



Uy ~ l v z 

mzy — rn y z 

n z y - n y z 



•or 

n x iv y 

l x z — l z x 

m x z — m z x 

n x z — n z x 



l y x - l x y 

m y x — m x y 

n y x - n x y 



'r 'u 



l x 

m x 



n„ 



U 

m z 



o. i. • ■ (2,16) 

bince the twist is a specific representation of a screw, it too can be defined 

in screw coordinates relative to different reference frames. Suppose a twist 

t' is defined relative to a frame O'X'Y'Z'. Again, suppose this frame is, in 

turn, defined with respect to another frame OXYZ. The representation t of 

the twist t' in the frame OXYZ is given by 



t = TV, 

where T is given in equation 2.14 and conversely, 

t' = T _1 t, 



(2.17) 



(2.18) 
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z \ 




S,S' 




~Y 



Figure 2.1: Although the screw is the same no matter which reference frame is 
used to define it, it is helpful to be able to transform the representation from one 
frame to another. Suppose a screw s' is defined in screw coordinates relative to 
a reference frame O'X'Y'Z' and O'X'Y'Z' is, in turn, defined relative to another 
frame OXYZ. Then the screw can also be represented in the OXYZ frame by a 
simple linear transformation 3 = TV. 



where T x is given in equation 2.16. Similarly for the wrench, given in screw 
coordinates relative to O'X'Y'Z', its representation in OXYZ is given by 



w = Tw', 



(2.19) 



where T is given in equation 2.14. Conversely, the wrench defined in the 
OXYZ frame may also be represented in the O'X'Y'Z' by 



w' = T _1 w, 



(2.20) 



where T x is given in equation 2.16. 



2.3 Contact types 

The interface between a robot and an object can be characterized by a par- 
ticular type of contact. For a three dimensional manipulator, there are, in 
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general, nine different types of contacts. These contact types are illustrated 
in figure 2.2, [Salisbury]. For each of these contact types, a particular set 
of wrenches may be exerted through the interface. The set of all possible 
wrenches that may be transmitted through a contact is known as a wrench 
system. A set of unit basis wrenches can be specified which span the entire 
wrench system. The nature of the contact problem, however, is inherently 
non-linear, since objects can both make and break contact with one another. 
Therefore, to describe the wrench system, it is necessary to define a set of 
unidirectional unit basis wrenches. Any wrench in the wrench system may 
then be represented by a positive linear combination of these unidirectional 
basis wrenches. Figure 2.2 lists, along with each contact type, the set of 
unidirectional unit basis wrenches whose positive linear combinations span 
the space of all the possible wrenches which can be transmitted through the 
contact. 

For the analyses in this thesis, only contacts made between the fingertips 
of the manipulator and an object will be considered, although the techniques 
developed here encompass other types of contacts as well. By considering 
only fingertip contacts, however, the number of possible contact types is 
reduced to four: a soft finger contact, a point contact with friction, a point 
contact without friction, and no contact. For convenience, these contact 
types will be represented by the following numbers 

1 = Soft finger contact 

2 = Point contact with friction 

3 = Point contact without friction 

4 = No contact. 



2.4 Infinitesimal motion 

Virtual work is the work done by a wrench exerted against an arbitrary 
twist. By examining the sign of the virtual work, we can determine the set 
of infinitesimal motions which are possible for an object constrained by a 
contact. First a contact is characterized by a particular contact type. Then 
the contact is replaced by a set of unidirectional unit basis wrenches, listed 
in the figures 2.2 and 2.3. Then for each basis wrench, the virtual work 
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Unit Basis Wrenches 



to = [1,0,0,0,0,0] 
to = [0,1,0,0,0,0] 

w = [0,0,0,1,0,0] 
to = [0,0,0,0,1,0] 
to = [0,0,0,0,0,1] 



to = [-1,0,0,0,0,0] 
to = [0,-1,0,0,0,0] 
to = [0,0,-1,0,0,0] 
w = [0,0,0, -1,0,0] 
to = [0,0,0,0,-1,0] 
to = [0,0,0,0,0,-1] 



Plane contact with friction 




Line contact with friction 




" = [1,0,0,0,0,0] «, = [-1,0,0,0,0,0! 

- = [0,1,0,0,0,0] u,- [0,-1,0. 0,0,0] 

w = [0,0,-1,0,0,0] 

to = [0,0,0, 1,0,0] w = [0,0,0, -1,0,0] 

to = [0,0, 0,0, 0,1] w = [0,0,0,0,0,-1] 



to = [1,0,0,0,0,0] to = [-1,0,0,0,0,0] 

w = [0,1,0,0,0,0] w = [0,-1,0,0,0,0] 

to = [0,0,-1,0,0,0] 



to = [0,0,0,0,0,1] to = [0,0,0,0,0,-1] 



w = [1,0,0,0,0,0] to = [-1,0,0,0,0,0] 
to = [0,-1,0,0,0,0] 
to = [0,0,1,0,0,0] to = [0,0,-1,0,0,0] 
to = [0,0,0, 1,0,0] to = [0,0,0,-1,0,0] 
to = [0,0,0,0,1,0] to = [0,0,0,0,-1,0] 



Plane contact without friction 
Figure 2.2: In general, there are nine different types of three dimensional con- 
tacts. Each contact type allows only certain wrenches to be transmitted through 
the interface. Since the contact problem is inherently non-linear, a set of unidi- 
rectional basis wrenches is defined whose positive linear combination describe the 
space of wrenches which can be transmitted through the interface. 
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Point contact with friction 
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Line contact without friction 
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Figure 2.3: Contact type* ( 
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is calculated. If the virtual work done by a twist against every unit basis 
wrench in the set is greater than or equal to zero, then the twist is allowed. 
Conversely, twists which produce negative virtual work for even a single basis 
wrench are disallowed. 

A complete derivation of virtual work is presented in [Ball] and the deriva- 
tions in this section are outlined in [Ohwovoriole]. A robot manipulator A 
exerts a wrench of magnitude a and pitch p a along a screw A on a body B, 
as shown in figure 2.4. The body then undergoes a twist of amplitude and 
pitch pp along a screw B. Define a coordinate system OXYZ with the x-axis 
aligned with the twist axis, the z-axis the common perpendicular between 
the twist and wrench axes, and the y-axis perpendicular to both the x and z 
axes, as shown in figure 2.5. The wrench can then be decomposed into forces 
and moments on the cartesian coordinate system 

F x = a cos $ 

F y = a sin 

F z = 

M x = a(p a cos - d sin 0) (2.21) 

My = a(p a sin + dcos 6) 

M z = 0, 

where d is the length of the mutual perpendicular between the wrench and 
the twist axes. The virtual work done by the wrench against the twist is 

W = a/Scos[(p a +pp)cosd - ds'md]. (2.22) 

The virtual coefficient between screws A and B is defined to be 

w = coe[(p a + p )coB0-dsm0]. (2.23) 

The virtual coefficient is independent of both the amplitude of the twist and 
the intensity of the wrench. Let (h, t 2 , t 3 , t 4 , t 5 , t 6 ) and ( Wl , w 2 , w 3 , w 4 , w 5 , w 6 ) 
be the screw coordinates of a unit twist and a unit wrench. The virtual 
coefficient is then 

u = w x t A + w 2 t 5 + w 3 t 6 4- w 4 ti + w 5 t 2 + w 6 ti. (2.24) 

We will use the unit twist and unit twist from now on when evaluating 
the virtual coefficient. The reason for this will be apparent later when we 
consider the value of the virtual coefficient. 
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Figure 2.4: The contact forces between a manipulator A and a body B can be 
modeled as a wrench of w along a screw A of magnitude a and pitch p a . The 
motion of the body can be described by a twist t along a screw B of amplitude 
and pitch pp. 




t(0,Pff) 



Figure 2.5: The wrench can be decomposed onto a coordinate system OXYZ. 
The z-axis is common perpendicular between the twist and the wrench axes, the 
x-axis is the twist axis, and the z-axis is perpendicular to both the x and the z 
axes. 
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2.4.1 Reciprocal screws 

Two screws are said to be reciprocal screws if the virtual work between a 
twist and a wrench associated with the two screws is zero. No work will be 
done by a unit twist t against a unit wrench w if the virtual coefficient is 
equal to zero. That is, 

u = W!t 4 + w 2 t 5 + w 3 t 6 + w 4 t! + w h t 2 + w 6 ti = 0. (2.25) 

2.4.2 Repelling screws 

Two screws are said to be repelling if the virtual work between the screws of 
a twist and a wrench is positive. Therefore two unit screws of a unit twist 
and a unit wrench will repelling if the virtual coefficient if positive. That is, 

u = w x t A + w 2 t s + w 3 t 6 + w A t x + w s t 2 + w 6 t! > 0. (2.26) 

Work is defined to be positive if the body moves in the same direction as the 
wrench. 

2.4.3 Contrary screws 

Two screws are contrary if the virtual work between the screws of a twist 
and a wrench is negative. Again two unit screws are contrary if the virtual 
coefficient if negative, 

u = wiU + w 2 t s + w 3 t 6 + w 4 t! + w 5 t 2 + w 6 h < 0. (2.27) 

2.4.4 Permissible twist 

A permissible twist is an infinitesimal motion that the constrained object is 
allowed to undergo. Assume an object is constrained by a single contact, as 
shown in figure 2.6. Also assume that the contact can be characterized by 
a particular contact type. The constraint imposed by the contact can then 
be represented by its associated set of unidirectional unit bases wrenches as 
was described earlier. 

For convenience, set of bases wrenches is defined in terms of a contact 
frame. The contact frame is a coordinate frame whose origin is the point of 
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contact between the object and the manipulator. The z-axis is the surface 
normal at the contact point, and the x and y axes lie in the tangent plane. 
In general, the choice of the x and y axes of the contact frame is arbitrary; 
however, a convention for resolving the ambiguity is given in appendix A. 
This convention, though useful for the implementation, is unimportant in 
the current discussion. Figure 2.6 then shows the contact frame Ci X c . Y Ci Z Ci 
defined relative to a reference frame OXYZ. 

Let {w c .} be the set of unidirectional unit bases wrenches which replace 
the contact type defined in terms of the contact frame. Their representation 
in the reference frame OXYZ is given by 



wj = Tiw Cj , 
where Ti is the transformation matrix given in equation 2.14 by 



(2.28) 



Ti = 



hy -l y z 

y ~~ *xV 






n x 
n y 



ly 

. h rn 
m z y- 
m x y — m z x 

m y y ~ mxV 



rriyZ 



n z y - n y z 
n x z — n z x 
n y x — n x y 



I 







m x 
m. 



n, 



n 2 



(2.29) 

where 1 = [l x , l y , l z ], m = [m x , m y , m z ], and n = [n x , n y , n z ] are the direction 
vectors of the x,y, and z axes and x, y, and z is the origin of the i th contact 
frame defined with respect to the reference frame OXYZ. 

The set of twists which are reciprocal or repelling to a single unit basis 
wrench wj is 



Tj = {t : ^1^4 + w 2j t 5 + w Zj t 6 + w^^ + w 5 .t 2 + w 6j t 3 > 0}. 



(2.30) 



The set of twist reciprocal or repelling to all the unit basis wrenches in Wj is 
the intersection of all the sets Tj 



T inf = f]{T 1 ,T 2 ,...}. 



(2.31) 



The set T; n y represents the set of twists which are either reciprocal or 
repelling to every unidirectional unit basis wrench of a single contact. The 
set Ti n f describes only infinitesimal motions which the object is allowed to 
undergo while constrained by a single contact. In order to determine the 
possible motions of an object finite motions must be considered. 
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Figure 2.6: The motion of an object is constrained by a single contact. The 
contact can be characterized by a contact type which can then be represented by 
a set of unidirectional unit basis wrenches {wj}. The set of all twists which are 
reciprocal or repelling to every basis wrench, describes the infinitesimal motions 
the object is allowed to undergo. 

2.5 Finite motions 

The set of twists T inf given in equation 2.31 describe the infinitesimal mo- 
tion of an object constrained by a single contact. In order to be useful 
in determining all the possible ways an actual object can move within a 
grasp, the definition of permissible motion must be extended to include fi- 
nite motion. Given an arbitrary unit twist t defined in screw coordinates 
t = [*i, t 2 , ^3, £ 4 , £5, t 6 ] relative to some reference frame, let 

h 

h 

U 

h 

its 

represent a finite motion, where mti,mt 2 , and mt 3 , are finite rotations and 
mt 4 ,mt 5 , and mt 6 , are finite translation along the ar, y, and z axes of the ref- 
erence frame. In general, the twists in the set T inf describe finite as well as 



t = m 



(2.32) 
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Figure 2.7: Surface geometry in the neighborhood of the contact must be con- 
sidered to determine the complete set of small finite permissible motions. 



infinitesimal motion, except for one important exception. Twists reciprocal 
to the wrench applied by a point contact without friction may or may not be 
permissible. Whether motion is permitted depends on the surface geometry 
in the neighborhood of the contact. Consider the following example of two 
dimensional objects constrained by two point contacts without friction, as 
shown in figure 2.7. The location and the orientation of the contacts are 
identical, thus the set of infinitesimal permissible twists for each object is 
the same. The finite constraint imposed on the objects, however, is different. 
The first object, with concave surfaces, is completely constrained in all di- 
rections. The second object, the rectangle, can only move horizontally, and 
the last object, the circle, can both translate or rotate between the contacts. 
The difference in constraint comes not from the contact types, but from the 
curvature of the surfaces. In order to determine the complete set of permis- 
sible finite motions, the surface geometry in the neighborhood of the contact 
must be considered. 

Therefore, we want to determine the motions which do not violate the 
geometric constraints imposed by the surface of the object. To solve this 
problem, assume an object undergoes a hypothetical finite motion describe 
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by a twist mt, in equation 2.32, then determine whether this particular 
motion causes the contact point to penetrate the surface of the object, thus 
violating the geometric constraints. Let the surface of the object be describe 
by a function f S urf ace (x,y) relative to the contact frame Cl „ X Ci , Y Ci , Z Ci . 
First, determine the motion of the contact point relative to this contact 
frame as the object undergoes the finite motion describe by mt. Second, map 
the trajectory of the contact point onto the surface of the object. Finally, 
compare the trajectory of the contact point with its projection onto the object 
surface to determine whether this particular motion violates the geometric 
constraints. 

When comparing the motion of the surface of the object relative to the 
contact points, it is unimportant whether we define the motion as a twist 
mt that the object undergoes relative to fixed set of contact points or as a 
twist -mt that the contact points undergo relative to a fixed object. For 
convenience, we will assume the object remains fixed relative to the OXYZ 
frame and the contact points move with a twist -mt. The trajectory of the 
contact point is given by 

x t = T tXi + d, (2.33) 

where Tt is a matrix 



T t = 



hhcim + hs m 

Ml Clm — t 2 S m 



hhcim 



hhClm — h^m 



(2.34) 



where c lm = (1 - cos(m)), c m = cos(m), and s m = sin(m). T t multiplied by 
Xi yields the displacement due to a rotation, while 



d = 



mt 4 


mt 5 


. mt e . 



(2.35) 



is a displacement due to translation. Equation 2.33 describes the displace- 
ment of the contact point relative to the OXYZ frame, but we want to 
know the displacement of the contact point relative to the contact frame 
Ci X Ci Y Ci Z Ci . Therefore, let x tra j be the trajectory the contact point relative 
to the contact frame. Then x tra j is given by 



x traj — T r .xt, 



(2.36) 
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where T r . is a rotation matrix relating points in the contact frame C . , X Ci , Y Ci , Z Ci 
to points in the reference frame OXYZ, 



T r .= 



m x 
ro„ 



/ 



L m 



n, 



n, 



(2.37) 



Equation 2.36 describes a locus of points relative to the contact frame. 
These points represent the trajectory of the contact point would undergo if 
the object were displaced by a twist mt. For a given twist, the trajectory of 
the contact point is a function of only one variable, the magnitude m 



x traj 



x traj 




' fx(m) ' 


Vtraj 


= 


fv( m ) 


. Zfraj 




. Mm) . 



(2.38) 



By projecting the trajectory of contact onto the surface of the object, it is 
possible to determine whether a particular twist t describes a permissible 
finite motion. Figure 2.8 shows the motion of the contact point and its 
projection onto the surface. The surface of the object is described by the 
equation 

^surface = fsurface(x,y). (2.39) 

and the projection of the contact trajectory onto the surface of the object, 
is given by 



x proj — 



•Pproj 
Vproj 
Zproj . 



(2.40) 



fx{m) 
fv(™>) 

. fsurface[fx(m),f y (m)]. 

For a motion to be permissible, the contact trajectory must not penetrate 
the surface. Simply 

Ztraj > Z proj . (2.41) 

Therefore, the set of finite motions the object may undergo is 

Tfin = {t : Z traj (t) > Zproj(t)}. (2.42) 

Finally, the set of all finite motions of an object subject to a single constraint 
is the intersection of the finite and infinitesimal permissible motions 



T = Tf in f]T inf . 



(2.43) 



mm^m^*<»w^^mmf'mmK m jm m mmKtwmwi . y x\ \ mmmm ^ ! ^^9>^'s 



CHAPTER 2. CONSTRAINT 



22 




cootact trajectory 
rarfece txajectory 



Figure 2.8: The projection of the eenteet ftqjertery 
— "*** ~ v ^ k — rmrrfnitwr mntinw ii pnaitMa A 
to peaetrate the taste ii *9t alette* 
move away from the eaifece aie permitted. 



onto Mm object surface 
wafca ceases the coatact 
the object to 



CHAPTER 2. CONSTRAINT 23 

2.5.1 Value of virtual coefficient 

The definitions in the previous section provide mathematical constraints 
which work well for abstract models. An infinitesimal variation in the di- 
rection of the unit twist, however, will change its definition from reciprocal 
to repelling or contrary. In reality, small changes in constraint should not 
lead to drastic changes on the object. This problem occurs because a twist is 
defined to be reciprocal only when the virtual coefficient is exactly zero. In 
order to compensate for inaccuracies in contact measurement and to allow 
more generality in object motion, it would be helpful to define a range of 
values , -u> r to u> r , for which a twist is defined to be reciprocal. That is, a 
reciprocal unit twist satisfies 

- u r < w x t 4 + w 2 t 5 + w 3 t 6 + w 4 t x + w 5 t 2 + w 6 ti < uj r . (2.44) 

This, however, also changes the definition of the repelling and contrary twist. 
A repelling unit twist will now be defined as 

Wit 4 + W 2 t s + W 3 t 6 + W A t X + U>5<2 + «>6*1 > w r , (2.45) 

and a contrary unit twist 

w t t 4 + w 2 t 5 + w 3 t 6 + w 4 t! + w 5 t 2 + w 6 t! < -u> r . (2.46) 

For example, consider the case of two fingers grasping a rectangle in two 
dimensions, as shown in figure 2.9. If tactile sensors are used to resolve the 
contact point and surface normal, there is to be expected some small error 
in the reading, figure 2.9. It is obvious that the block can slide between the 
fingertips in either direction if the grasp is loosened sufficiently. The strict 
definition of the reciprocal twist, however, indicates only one or the other 
direction is possible, depending upon error in the sensed surface normal. By 
expanding the definition of the reciprocal twist, in equation 2.5, we can again 
obtain the correct result, that the rectangle can move in either horizontal 
direction. Now that the definition of the reciprocal twist, equation 2.23, has 
been expanded, the geometric constraints will become more important when 
determining possible motion. 
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Figure 2.9: Small errors in the definition of the surface normal of the contact 
on a two dimensional rectangle lead to large variation in the predicted physical 
system. By expanding the definition of the reciprocal twist, these errors can be 
accounted for as well as yield more intuitive results in determining the allowed 
motions of an object within a grasp. 
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2.6 Multiple contacts 

If there are m contacts between the manipulator and the object and n dif- 
ferent types of contacts, there will be n m possible combinations of contacts. 
Each combination of contacts yield a different set of constraints on the ob- 
ject. An object may be said to be in a particular constraint state defined by 
the number and the types of contacts. For m contacts and n contact types 
there are n m possible constraint states. For example, if a three fingered hand 
grasped an object at the fingertips there are three contacts and, since there 
are only fingertip contacts, there are four contact types. With three contacts 
and only four possible contact types there are 4 3 possible combinations, that 
is 64 constraint states. For grasps made with a human hand at the finger- 
tips, there are now five contacts, and therefore 4 5 or 2048 possible constraint 
states. A constraint state can be denoted by an ordered list of m elements, 
each element denoting the contact type. Suppose the three fingered hand 
has its fingers numbered one, two, and three, and assume the hand grasps an 
object at the fingertips. If the first and second fingers behave as soft finger 
contacts, while the third finger slips on the surface, the constraint state will 
be [1, 1,3]. Similarly, if a human hand grasped a cup, for example, and the 
thumb, index finger, middle finger, make soft finger contacts on the object, 
the ring finger slips on the surface, and the small finger is removed, the 
constraint state would be represented by [1,1,1,3,4]. More generally, the 
constraint state is denoted by 

C = [ Cl ,c 2 ,...,c m ], (2.47) 

where c, is the contact type for the i th contact. 

2.7 Permissible Motion 

From the previous analysis, the set of permissible motions for an object 
constrained by a single contact was given by T, equation 2.43. Now assume 
the object is constrained by multiple contacts and let the set of permitted 
motions for each contact be given by T [ci] , where c, represents the contact 
type for the i th contact. 

If there are m constraints on the object, the set of permissible twists for 
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Chapter 3 

Strategies for manipulation 



3.1 Introduction 

In this chapter, we will examine different external forces that can act on 
an object and how they can be used to move the object around in a grasp. 
An object may be influenced by a number of other forces besides the grasp- 
ing fingers of a robot. Gravity, acceleration, electro-magnetic forces, other 
objects, and free fingers can all exert forces on the object. Although these 
forces differ, they can all be collected into a single external wrench acting on 
the object. The object will then tend to undergo a twist repelling to this 
external wrench. The set of twists repelling to the external wrench can be 
called the preferred twists, since they describe the possible motions the object 
can undergo. The actual motion, however, will depend on the constraints on 
the object. That is, in order for object to actually move, the set of preferred 
twists must intersect the set of permissible twists, discussed in chapter 2. 

We will begin by examining a general strategy for using the external 
wrench to move an object in a grasp. Then we will examine different external 
wrenches including gravity, controlled accelerations, and forces produced by 
the robot and the environment. For each of these external wrenches, different 
controlled slip manipulation strategies will be investigated. 
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3.2 General strategy 

Assume a grasped object is subject to an external wrench w e . The object 
will then tend to undergo a twist repelling to this external wrench. The set 
of repelling twist, or preferred twists, is given by 

T p = {t|u> ei *4 + w ei h + w e3 t 6 + w u t! + w ei t 2 + w e6 t 3 > 0}. (3.1) 

In order for the object to actually move, however, the set of preferred twist 
must intersect the set of permissible twists. That is, 

T = T p f]T [ci Cm] , (3.2) 

where T[ Cli ... )Cm ] is the set of all possible motions the object may undergo while 
in the [ci, . . . , c m ] constraint state. 

The set of twists described in equation 3.2 is of particular interest, since 
this set will describe the way the object will actually move. If T = 0, then 
the object is completely constrained and will not move at all. If T contains 
a single twist t, then the object will move in a direction described by this 
twist. This case is especially important in controlled slip manipulation, since 
the geometry constraints and the external wrench together specify a unique 
motion of an object within the grasp. However, if T contains more than a 
single element, the resulting motion cannot be determined from the geometric 
analysis alone. Dynamics, external forces, internal grasping forces, and local 
surface friction properties must all be taken into account to determine the 
exact object motion. Although this type motion is difficult to control even 
for humans, it should not be ignored in robotic manipulation, since it may 
be useful in predicting motion resulting from inadvertent slipping or more 
complex dexterous motions. In fact, geometrically unconstrained motion was 
analyzed by [Mason], in which he predicted the motion of an object sliding 
in a plane while subject to a specific velocity at a single point. 

The objective of controlled slip manipulation is to control the motion of 
an object relative to the grasp. There are some motions which are easy to 
accomplish through controlled slip manipulation. These are motions which 
are both allowed by the constraints on the object and bound by the geometry 
contacts. In other words, if the intersection of the permissible and preferred 
twists, equation 3.2, is a small bounded set of the twists, then these motions 
which can be easily implemented by a robot. Therefore, when planning 
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controlled slip motions, consideration should be given to these constrained 
twists as well as the twists which are particularly desirable. In any case, a 
desired twist is selected td, describing the motion the object should undergo 
in the grasp of the robot. In order to facilitate this desired motion, the 
external wrench on the object should be optimized so that it will move the 
object through the desired twist. The optimal external wrench is one in the 
virtual work of the wrench against the desired twist is a maximum. These 
external wrenches are 

{w e |m<xa;{u;(we,td)}}. (3.3) 

In the following sections, we will examine different ways to exert an ex- 
ternal wrench on a grasped object and how these external wrenches may be 
used to control the slipping motion of an object within a grasp. 

3.3 Specific strategies 
3.3.1 Gravity 

People use gravity to their advantage when manipulating objects. We repo- 
sition objects in our hands by allowing then to drop, slide, or rotate between 
our fingers. Consider, for example, when a glass of water is raised from the 
table. We can allow the glass to rotate between our fingertips so that the 
glass remains vertical. Relative to our hand, gravity has been used to rotate 
the glass and in this way maintain the vertical orientation. Gravity can also 
be used by a robot manipulator to reposition objects within a grasp. The 
force imposed by gravity can be modeled as a single force at the centroid of 
the object. Hand orientation can then be used to allow gravity to move the 
object through a desired twist. 

Suppose we wish to use gravity to move an object within a grasp. First, 
we specify a desired twist, t d , that is, a twist, relative to the grasp, through 
which we want the object to move. Then, by reorienting the object, the 
external wrench w e relative to the grasp, can be varied. The virtual work 
created by the external wrench against the desired twist is a function of the 
object orientation. It is then possible to determine a set of object orientations 
for which the virtual work is a maximum and at these orientations gravity 
will be optimally used to move the object. 
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To illustrate how gravity might be used, suppose a desired twist t d is 
specified in screw coordinates relative to an object frame X o Y Z . The 
object frame is a coordinate system whose origin is the centroid of the object 
and whose axes are fixed within the object. Now let the object frame be 
defined in terms of the hand frame frame O h X h Y h Z h , where the z axis of 
the hand frame is parallel to gravity, figure 3.1 and the x and y axes are 
defined for a specific hand. The hand coordinate system for the Salisbury 
robot hand is described in appendix A. In this example, suppose the axes of 
the object frame are initially aligned with the axes of the hand frame. The 
wrench caused by gravity acting on the object defined relative to the object 
frame is 



w = 



" w l ' 




"0 


w 2 







w 3 

W4 
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-mg 



w$ 







. «>6 . 




. 



(3.4) 



where g is the gravitational acceleration and the m is the mass of the object. 
However, if the object frame is rotated, the value of the external wrench 
defined with respect to the object frame changes. The Salisbury robot hand 
has the ability to rotate the object through arbitrary angles about the axes of 
the hand frame. Therefore, assume the object frame is rotated by an angle 9 
about the x axis and then by an angle (j> about the y axis of the hand frame. 
Now the external wrench in terms of the object frame will be 

— mg cos 9 sin <f> 

—mgs'mO 
— mg cos 9 cos <f> 







Only two rotations are used in equation 3.5, since rotations about the z-axis 
of the reference frame produce no change in the wrench. 

For certain values of 9 and <f> the value of the virtual work is a maximum. 
At these orientations, the axes of the gravitational wrench and the desired 
twist are the same. Given the virtual work 
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(3.5) 



w = -mg(cos 9 sin <}>t c< + sin 9t Ch + cos 9 cos <f>t C6 ), 



(3.6) 
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Figure 3.1: The wrist can be used to orient the object in a grasp so that 
gravity can be used to move the object 



the values of 9 and <f> which produce a critical values of virtual work are 
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= sin 
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= cos 
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(3.7) 



Substituting the values for 9 and <f> into equation 3.6 yield either positive or 
negative values for the virtual work. Values of 9 and <f> for which the virtual 
work is positive, produce the maximum value of virtual work. 

Therefore, by using the wrist to rotate the hand about the angles given in 
equation 3.7, the robot can use the gravitational force to maximum efficiency 
when moving an object within the grasp. The use of gravity can be an 
effective means for controlling the motion of an object within the grasp. 
However, it may also be somewhat difficult, since the external force cannot 
be controlled; all the control must come from the wrist and the fingertips. 
The manipulator must also respond quickly to the motion of the object, 
since once it begins to slide it will continue to slide. Other methods, such 
as, controlled accelerations and point forces from both non-grasping fingers 
and external objects, can be used as a more controlled means of producing 
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an external wrench on the object. 

3.3.2 Controlled accelerations 

It is possible to accelerate hand to create a force on an object. For example, 
we loosen our grasp and flip an object in our hands, we have used an ac- 
celeration to create a force on the object to produce a desired motion. The 
strategy is to create an external wrench on the object while adjusting the 
grasp so that the object is in a constraint state which will allow it to slip. 

Using the definitions in the previous section, we will find the direction 
the hand should be accelerated to move the object through a desired twist. 
Again, assume a desired motion is specified by a twist t^ defined in screw 
coordinates relative to the object frame. To maximize the virtual work, the 
direciton of the acceleration, in terms of the object frame, should be in the 
same direction as the twist. Simply, 



a = 



" O-B 




td! 


a^ 




U 2 


a^, 
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U 3 
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_ |td| 
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Us 
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. U<> . 



(3.8) 



Although, the use of controlled accelerations is one way to move an object 
within the grasp, by far the most common and effective way to reorient an 
object within a grasp is the use of free finger, that is, fingers not directly 
involved with the grasp, and other objects. 



3.3.3 Free fingers 

Fingers not involved with the actual grasp can be used to move the object. 
This is very common in human manipulation. We can reorient a pen , for 
example, by holding it between two of our fingers and spinning it with a third. 
A multifingered robot can also use free fingers to manipulate objects. While 
some fingers constrain the object, the others have the freedom to reorient 
it within the grasp. This may also be an argument for multifingered hands, 
since the additional fingers allow greater flexibility in producing constraints 
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on an object as well as allowing the free fingers to manipulate object within 
the grasp. 

Suppose the surface of the object can be represented by a set of points S 
and for each of the i free fingers there is a subset of S of accessible surface 
points, S{ C S. 

Figure 3.2 shows an object located with respect to a reference frame 
OXYZ. For each point x € Si, a specific set of forces can be exerted through 
the contact. Assume the forces lie within the friction cone at the contact. 
That is, the set of wrenches which can be exerted in terms of the contact 
frame Ci X Ci Y Ci Z Ci , are 

{*}\y/v>Z+< < /iv/^Q, (3.9) 

where // is the coefficient of friction. The set of wrenches defined relative to 
the reference frame is 

{w e = T iWj }. (3.10) 

The wrenches in equation 3.7 are a combination of both the accessible surface 
points which the free finger can reach and the forces which it can exert 
through each contact point. Suppose a twist is defined tj defined relative to 
the reference frame OXYZ. The values for the virtual work of the twist t d 
against the wrenches in 3.7 is 

W = {u\uj = w ei t d4 + w e2 t di + w e3 t de + w e J dl + w ei t di + w et t d3 }. (3.11) 

The combination of wrenches and accessible surface points which maximizes 
the virtual work can be expressed as a set of pairs 

{(w,x)|(w,x)€max{^}}. (3.12) 



3.3.4 Other objects 

Other objects can also be used effectively to control the motion of an object 
within a grasp. In the jar example, given in the introduction, we use the 
edge of the jar to rotate the lid between our fingertips. A robot could also 
exploit objects in the environment to reposition an object within a grasp. 
The strategy and the analysis for this type of manipulation are identical to 
the free finger analysis. Only the accessible surface points 5,- are different, 
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Figure 3.2: A force exerted on a grasped object out move it through a desired 
motion 
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Chapter 4 
Grasping force 



4.1 Introduction 

There are many ways for a multifingered hand to squeeze an object. People 
grasp objects to secure them in their hand or, by carefully controlling the 
internal forces, allow objects to slide through their fingers. A robot can also 
use the internal grasping force to create both stable and unstable grasps. By 
regulating the grasping force, the robot can either hold the object securely 
or by control the constraint state to allow desired slipping motion. 

The variety of ways a hand can squeeze an object depends on the number 
and type of contacts which exist between the hand and the object. The goal 
of this chapter then is to determine the space of possible grasping forces on 
an object and to develop a simple intuitive parameterization of the squeezing 
force space for two and three fingered grasps. 



4.2 Grasp force analysis 

Assume the manipulator exerts a wrench on the object 



w gi = 



w 
w, 
w { 

w l 

w, 



w, 



(4.1) 
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defined in screw coordinates relative to the contact frame O gi X Ci Y Ci Z Ci . Sup- 
pose that the contact frame can be defined in terms of a common reference 
frame OXYZ, as shown in figure 4.1. The wrench in terms of the reference 
frame is 

wi = TiWg., (4.2) 

where Ti is the transformation matrix discussed in chapter 2. The sum of 
the wrenches from n contacts between the robot and the object is 

n 

= £ w i- (4.3) 



w 

»=i 



Assume the object is not subject to any external wrench and is not acceler- 
ating. The sum of the wrenches in equation 4.3 is then zero, 

w = 0. (4.4) 

One additional constraint will be assumed. It is assume that only forces 
through a point contact contribute to the internal grasping force. The mo- 
ments at the contact point will be assumed to be zero, so that the wrench, 
in terms of the contact frame, is given by w g . = [w gi ,w g2 , w g3 , 0,0,0]. With 
these assumptions, it is now possible to determine the wrench at each con- 
tact point relative to the contact frame, as a function of the internal grasping 
force. 



4.3 Two contacts 

Assume the robot touches an object at only two points. Therefore the sum 
of these wrenches, equation 4.4, is 

T i w gi+T 2 w g2 =0. (4.5) 

There are six unknowns in equation 4.5. These are the wrenches, W]. = 
[w gll , w g2l , w g3l , 0, 0, 0] and w 2 = [w gU , w g22 , w g3i ,0,0,0]. However, equation 
4.5 yields only five linear independent equations. Therefore, there is a one 
space of solutions for which equation 4.5 is true. 

The one space of solutions can be thought of as an arbitrary squeezing 
force, given simply by a grasp force magnitude m g . Once the grasp force 
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Figure 4.1: Assume each of the grasping fingers can exert a wrench w c . defined 
in terms of the contact frame through the contact point. The wrench defined 
with respect to the common reference frame is wj = Tjw c ., where Tj is the 
transformation matrix discussed in chapter 2. The sum of each of the wrenches 
is w = 22?=i w i and if the object is neither subject to external wrenches nor 
accelerating, this wrench w is zero. 
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magnitude is specified, the values of the wrenches w g and w g _ can be 
found. 



Let the grasp force magnitude m g be given by 



ro. 



= Evv 



t=i 



»i, 



+ < 



+ w\ 



g^i 



(4.6) 



The screw coordinates of the wrench due to internal grasping force can be 
found more easily by considering the wrenches in terms of an axis, a pitch, and 
a magnitude. Assume the two contact forces are described by wrenches wi 
and w 2 , defined by wrenches axes Ai and A 2 , zero pitches, and magnitudes 
mi and m 2 respectively. Since the object is neither subject to an external 
wrench nor accelerating, both the wrench axes must intersect the contact 
points and have opposite directions, figure 4.3. The wrench axes are given 

by, 

(xx - x 2 ) 
(yi - to) 

-*i(yi - Vi) + »i(*i - z 2 ) 
z i (x 1 - x 2 ) - Xi(zx - z 2 ) 
-Ui(xi - x 2 ) + xi(yi - y 2 ) . 

where d is the distance between the contact points 



± Ai = TA 2 = 



(4.7) 



d = yj{x x - x 2 f + (y x - y 2 f + ( Zl - z 2 f . 



(4.8) 



Notice the direction of the axes in equation 4.7 are opposite, yet the value 
of one of them may be either positive or negative. This analysis does not 
determine the sign in equation 4.7, since this will depend on the physical 
system. For example, if two fingers grasp a block, the wrench axes would 
be directed into the block, but if the fingers grasp inside a ring, the wrench 
axes would be directed outward. Therefore the direction of the wrench axes 
will depend on the direction of the contact normal. The direction of the axis 
which yields a negative value of the dot product between the contact normal 
and the wrench axis will be the correct direction. 

The sum of the individual magnitudes is given in equation 4.6 



m g = m i + "^2- 



(4.9) 
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The magnitudes of the wrenches, however, must be equal, since if one ex- 
ceeds the other, there will be a net wrench on the body contradicting the 
equilibrium assumption; thus, 



mi = m 2 . 
Then by equation 4.9 and 4.10 

mi = m-i = rrig/2. 



(4.10) 



(4.11) 



The wrenches w x and w 2 are now uniquely defined. The wrench axes are 
given in equation 4.7, the magnitudes in equation 4.11, and the pitch of both 
wrenches is zero. 

It is useful, in the later analysis, to express the wrenches in terms of a 
reference frame and the contact frame. The wrenches Wj defined in terms of 
screw coordinates relative to the reference frame are 



m„ 



i "'0 

Wl = -W 2 = ±-i 



(Xi — x 2 ) 

(yi - ife) 

(*i - z 2 ) 

-*i(yi -V2) + yi(zi -z 2 ) 

Zl(Xi - x 2 ) -x^zi - z 2 ) 
. -Vi(xi - x 2 ) + xi(y! - y 2 ) 



(4.12) 



The screw coordinates of the wrenches in terms of the contact frame is a 
linear transformation of the elements in equation 4.12, 

w gi = T I lw i (4.13) 

w g 2 = T 2 lw 2, (4.14) 

where Tj~ is the inverse of the linear transformation matrix discussed in 
chapter 2. 



4.4 Three contacts 

For a three fingered hand, such as the Salisbury or Okada [Okada] robots, 
three fingertip contacts are possible. Assume the robot touches the ob- 
ject at three points and can only exert forces through the contacts, then 
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the wrench at each contact denned in terms of the contact frame is w g . = 
i w gu , w g2i , w 3 3i , 0, 0, 0] and the wrench, in screw coordinates, defined in terms 
of a reference frame is 

wj = Tiw g .. (4.15) 

The sum of the wrenches, from equation 4.4, is 

Tiw gl + T 2 w g2 + T 3 w g3 = 0. (4.16) 

There are nine unknowns in equation 4.16, w x = [w gll ,w g2l ,w g3l , 0,0,0], 
w 2 = Ki 2 , ^22,^32,0,0,0], and w 2 = [w gli ,w g22 ,w g32 , 0,0,0], and equa- 
tion 4.16 yields only six independent equations. This still leaves three inde- 
terminant variables. The three space of possible solutions can be represented 
fairly simply. The wrench axes of each of the three wrenches will, in general, 
intersect at a point, the grasp force focus. The grasp force focus must lie on 
the grasp plane, defined to be the plane containing the three contact points. 
In general, the grasp plane will be well defined; however, if the contact points 
lie along a single line, the plane collapses to a line and the grasp focus lies on 
this line. If the grasp plane exists, the grasp force focus, may lie anywhere 
on the plane. As long as there is no external wrench on the object and the 
object is not accelerating, the wrenches will intersect at a point, with one 
exception: the wrench axes may all be parallel and still be in equilibrium. In 
this case, the grasp force focus parameterization can still be used if we also 
consider points at infinity on the grasp plane. 

There is a third variable, the grasp force magnitude m g . The grasp force 
magnitude is a scaling factor by which individual wrench values are multi- 
plied. Together the grasp force focus and the grasp force magnitude span 
the entire three space of internal grasp solutions. By choosing a grasp force 
focus x g y g on the grasp plane and with a grasp force magnitude m g , we can 
uniquely define the internal squeezing force created by a three fingered grasp. 
To find the wrenches resulting from this parameterization of the grasp 
force, we first find the wrenches in terms of the grasp frame. The grasp 
frame is the coordinate system whose x and y axes he on the grasp plane and 
origin is fixed at the centroid of the three contact points. Let wj represent the 
wrench defined relative to the grasp frame xj = [x' it y' iy 0] represent a contact 
point. The wrench axes Aj of each of the wrenches will be the direction 
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vectors from the contact points xj to the grasp force focus x g . 



Ai = ± W\ 



Xg X i 

y 9 - yl 
o 

o 

o 

-y'i(x g - x' { ) + x'i(y g - y'i) 



(4.17) 



where |J,| = yj{x g - x<) 2 + (y g - y<) 2 . The sign used in equation 4.17 de- 
pends on geometry. If the fingers were grasping a sphere and the grasp force 
focus was in the center of the sphere, the sign of Aj would be positive. Con- 
versely, if the fingers were grasping a ring from the inside the sign of A[ 
would be negative. In general, the sign depends on the direction of the nor- 
mal to the contact. The sign should be chosen so that the wrench axis and 
the contact normal are in the same direction. Let the grasp force magnitude 
be the sum of the individual wrench magnitudes, m,-, 



m g = mi + m 2 + m 3 . 



(4.18) 



Since the wrench axes are given by A-, in equation 4.17, and the magnitudes 
are given by m,, the first two elements of the wrench in screw coordinates 
relative to the grasp frame are 



w{. = mi (x a - x$/\di\ 
"4 = rrti{y a - y,-)/|rf,-|. 



(4.19) 
(4.20) 
(4.21) 



By equation 4.4, the sum of the elements in equation 4.21 must sum to zero. 
Together with equation 4.18 this yields three linearly independent equations 
and three unknowns, 

(**-*i)/|rfi| (^-x' 2 )/\d 2 \ (x g -x' 3 )/\d 3 

(Vi-vl)/\di\ (vi-nti/fal (y g -y' 3 )/\d 3 \ 

1 1 1 

J (4.22) 
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Solve for the magnitudes m,-, 



m 2 
m 3 



(*,-*;)/|<fi| (x g -x' 2 )/\d 2 \ (x g -x' 3 )/\d 3 \ 

(v.-vi)/\di\ fa-iti/fal {v 9 -y'z)l\dz\ 

1 1 1 



-l 






m„ 



The matrix in equations 4.22 and 4.23 will be invertible except when the 
three contact points lie on a line and the grasp force focus does not. In this 
case, choose a grasp force focus on the line and then the magnitude will be 
proportional to the distance from the contact points to the focus, 



m, = m 5 |c?,| 



(4.24) 



In any case equation 4.17 defines the wrench axes, equations 4.23 and 4.24 
defines the wrench magnitudes, and pitch are all zero. The wrench resulting 
from the internal force at each contact is now defined by a grasp force focus 
x g and y g and a grasp force magnitude m g . 

The wrenches in screw coordinates relative to the grasp frame are 



w =± 



1*1 



Xg X i 

y 9 - y'i 

Z 9 ~ Z 'i 



-z'i(y 9 - y<) + y'i(z g - z\) 
z 'i(x g - *!•) - *(•(*, - *0 

L -y'i(x g - x\) + x'iiyg - y() J 



(4.25) 



and in terms of the contact frames 



w g . = !>;. 



(4.26) 



where Tj is the linear transformation relating the wrench in the contact frame 
to the grasp frame. 



4.5 Four or more fingered grasps 

For four fingered hand like the MIT/Utah hand, [Jacobsen], there are twelve 
unknowns and only six equations, yielding a six space of internal grasping 
force solutions. For the human hand, for manipulators with six or more 
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Figure 4.3: There is a three space of solutions for the internal grasping force for 
a three fingered hand. The axes of the force vectors from the individual contacts 
must intersect at a point, the grasp force focus x g = [x g , y g , 0] in the grasp plane. 
The grasp force magnitude m g scales the individual fingertip forces. Together, the 
grasp force centroid and the grasp force magnitude span the three space of internal 
grasp solutions. 



fingers, and for grasps involve more than five contacts, the dimension of the 
internal force space grows very large. The dimension of the solution space 
for internal grasp force is 



Dim{G) = 3(7V - 2) + U, 



(4.27) 



where G is the space of internal grasp solutions, TV is the number of contacts, 
and U is the number of freedoms of the grasped object. 



Chapter 5 
Contact wrenches 

5.1 Introduction 

In order to determine the different ways an object can move in a grasp, the 
constraint state of the object must be known. The constraint state of the ob- 
ject can be found knowing the location, orientation, and the types of contacts 
that exist between the object and the manipulator. A tactile sensor might 
supply contact location and orientation information, but a sensor which de- 
termines the contact type does not exist. In order to find the contact type at 
the fingertips of the robot, the contact wrench, that is the set of forces and 
moments which exist at the contact point relative to the contact frame, must 
be found, as well as, a relationship between the contact wrench to the con- 
tact type. The purpose of this chapter is to determine the contact wrench. 
The purpose of the next chapter will be to determine a simple relationship 
between the contact wrench and the contact type. In this way we can find 
the constraint state of the object as a function of the grasping force and the 
external forces. 

5.2 Stiffness 

Assume a hemispherical compliant fingertip touches a flat plate, as shown in 
figure 5.2. To begin the analysis, assume the interface between the fingertip 
and the object can be modeled as a soft finger contact. That is, both forces 
normal and tangent to the surface as well as moments about the surface nor- 
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mal can be transmitted through the contact point. A system of translational 
and rotational springs attached to the contact point will be used to model 
the fingertip, as shown in figure 5.2. The stiffnesses of the i th fingertip can 
be represented as a matrix relating the twist to the wrench 



k C! = 





kc\ 














kc 2 













kc 3 


kc 4 








kc 5 









kc 6 









(5.1) 



For a soft finger contact, the elements kc 4 and kc 5 are zero, that is, the 
fingertips cannot generate moments about any axis which lies in the tangent 
plane. However, for generality kc 4 and kc 5 are included, since linear and 
planar contacts have these stiffness and these contact types may be used in 
future analyses. 

The stiffness given in equation 5.1 is defined in terms of the contact frame 
Ci X Ci Y Ci Z Ci . The fingertip stiffness defined relative to a common reference 
frame OXYZ is 

ki = Tikc.Tr 1 . (5.2) 

where Ti is a transformation matrix of the i th contact discussed chapter 2 
equation 2.14. The stiffness of the entire object will be the sum of all the 
individual stiffness matrices 

K = Eki. (5.3) 

Assume the object is displaced by a twist t, the wrench on the object is given 

by 

w = Kt. (5.4) 

If the object is not accelerating, the wrench given in equation 5.4 must equal 
the external wrench on the object, w e , 

w = w e . (5.5) 

The twist the object undergoes can be determined as a function of the ex- 
ternal wrench on the object, by taking the inverse of K in equation 5.4, 



t = K 



-l 



w f 



(5.6) 
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Figure 5.1: The contact between the manipulator and an object is generalized 
as a hemispherical compliant solid touching a nondeformable flat plate. 

The twist in equation 5.6 is defined in screw coordinates relative to the 
OXYZ reference frame. The twist defined relative to the contact frame can 
be found by multiplying by the inverse of the transform matrix Tj, 

tj = TiK^We, (5.7) 

and the wrench at the contact, in terms of the contact frame, is 

wj = kc.TiK^We. (5.8) 

Add to this the wrench due to the grasping force 

Wi = kcjTiK^We + w g .. (5.9) 

Thus, equation 5.9 describes the wrench at the contact point defined relative 
to the contact frame as a function of the stiffness of the fingertips, the forces 
on the body, and the force the grasp. Using this result and the results in the 
next chapter, it will be possible to determine the contact type as a function 
of the forces on the object and the squeezing force. Once the contact types 
are found, the constraint state of the object will be known. 
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Figure 5.2: The fingertip is modeled as a set of translational and rotational 
springs connected to the contact point. The stiffnesses are defined in terms of the 
contact frame. Since a soft finger contact is assumed, there is a stiffness in the 
normal and tangent directions of the contact plane, as well as, a torsional stiffness 
about the contact normal. 



Chapter 6 

Contact wrench/contact type 
relation 



6.1 Introduction 

The purpose of this chapter is to determine a relationship between the contact 
type and the contact wrench. In general, this is a difficult problem. In fact, 
determining slipping and twisting of one compliant body in contact with 
another is a current research topic in finite element analysis. In this chapter, 
therefore, simplifying assumptions will be made to find a simple relation 
between contact type and contact wrench, so that the overall problem of 
determining the constraint state can be tractable. 

6.2 Two dimensions 

For the two dimensional problem, the relationship between the contact wrench 
and the contact type can be simple. Assuming the fingertips erert only forces 
though the contact points, there are only three contact types: a point con- 
tact with friction, a point contact without friction, and no contact. Assuming 
coulomb friction, if the tangent force is less than the coefficient of static fric- 
tion times the normal force, the contact will behave like a point contact 
with friction. If the tangent force exceeds the coefficient of friction times the 
normal force, the contact will slide, is modeled as a point contact without 
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friction. Given a wrench at the contact point denned by 



w = 



W 2 








(6.1) 



where w x force in the tangent direction, w 2 is the force normal to the surface. 
The possible contact types are 



1 = Point contact with friction 

2 = Point contact without friction 

3 = No contact, 



(6.2) 



and the relationship between the contact wrench and the contact types will 
be 

|t/>i| < \fiW 3 \ =» 1 

\wi\ > \fiw 2 \ =* 2 

w 2 > =» 3. 

For the three dimensional case, an exact relation between the contact type 
and the contact wrench is difficult; however, a simple relation can be useful. 



6.3 Three dimensions 

Determining when and where a three dimensional compliant contact breaks 
or twists in response to an applied wrench is currently at the forefront of 
finite element analysis. Exact solutions are, in general, unknown. The force 
distribution over the contact region between two hemi-ellipsoidal objects un- 
der an axially applied load can, be found by use of a linearization argument, 
commonly known as the Hertz contact problem [Lipson]. This simple case 
does not take into account loads applied tangent to the contact surface or to 
moments about the contact normal; however, we can use the Hertz solution 
to generate a simple relation between contact type and contact wrench. 

Hertz assumed two bodies in contact could be modeled as solid elliptic 
disks, each possessing a minimum and maximum radii of R and R'. Further- 
more, he assumed that these disks were in contact along their common axes 
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Figure 6.1: Two general solid elliptic disks in contact under an axially applied 
load. The load is assume small and the disks somewhat rigid, so that a linearization 
argument may be applied to determine the pressure distribution and the contact 
region. 

under a uniform axially applied load F, as shown in figure 6.3. Hertz deduced 
that the pressure distribution between the two bodies can be described by 
a semi-ellipsoid of pressure constructed over the surface of the contact, as 
shown in figure 6.3. 

The pressure distribution is given by 

P(x, V) = Poyjl - x 2 la? - y 2 /b*. (6.3) 

The total load, therefore, is equal to the volume of the semi-ellipsoid, 

2wabP 



Solve for P n 



F = 

3 

Po = — , 

27ra6' 



(6.4) 
(6.5) 
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Figure 6.2: Hertz modeled the pressure distribution in the contact area between 
the two solid elliptic disks as a semi-ellipsoid with a minimum and maximum 
dimensions a and 6. 



where a and b are given by Timosheko [Lipson] 



a = m/ 



b = n\ 



3FA 



4(A + 5)' 



3FA 



and 



^i>^2= Poisson's ratio 



4(A + BY 
A = (l-^)/£ 1 + (l-^)/£ 2 , 



(6.6) 
(6.7) 
(6.8) 



E 1 ,E 2 = Modulii of elasticity 



A + B =Hw+A+-k + -k) 
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* " -4 = h/(£ "£) + (£- i)' + 2 (i -£)'(£-£) cos(2,/,) 

V> = The angle between the planes contacting the curvatures l/Rx and l/R 2 

Ri,R[,R 2 ,R' 2 = Mimimum and maximum radii of curvature of the ellisoid disks at 
the point of contact 

m,n = Constants depending on B — A/B + A 



In the case of the contact between a hemispherical fingertip and a flat rigid 
plate, equations 6.6 and 6.7 is 



a = b = ^3/^ A/4, (6.9) 

and the maximum pressure is given by 



Po = °- 578 V rW- (6J0) 

If the flat plate is assumed to be rigid (i.e. E 2 > £"i) then A can be 
approximated by 

A = (l-tf)/E 1 . (6.11) 

The material on the fingertip of the Salisbury robot hand is a polyure- 
athane with an elastic modulus of approximately 40,000 psi. and a poisson's 
ratio of about 0.45. The radius of the fingertip is approximately 0.5 in, 
therefore 

A = 2.0 x 10" 5 
Po = 1247.9v / Flb/in 2 
a = 0.020#Fin. 
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The pressure distribution given in equation 6.3 can now be given in polar 
coordinates, since the pressure region in this case is given by the area of a 
circle. That is, 

P(r,d) = P ^~ll, (6.12) 

where r is the radial distance from the center of the contact region. Again 
assuming coulomb friction, the maximum moment the contact can support 
before rotating is the integral over the contact area of the differential force 
times the coefficient of static friction times the radial distance from the con- 
tact point, 

M = nJ A rdf. (6.13) 

Substitute these values into equation 6.13 

f r=a f e=2ir 3FVa 2 - r 2 , 
and evaluating yields 



., 3xuaF 
M = — - — . 
16 

Substituting the value for a 



M = 3tjiF J ZFRjl -7fl> 
16 V 4£?! 



(6.15) 



(6.16) 



Thus, M is proportional to the four-thirds power of the normal force, while 
the constant of proportionality is a function of constant properties of the 
fingertip material and the surface, 

M = fi m F^ 3 , (6.17) 

where 

_ top J 3R(1 - ^7 

^"ITV™!^ — • (6,18) 

If the specific values for the Salisbury robot fingertips are substituted along 
with a value of fi = 1.0 for the coefficient of static friction, the maximum 
twisting moment will be 

1.15 x 10~ 2 F 4/3 . (6.19) 
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Now ignore the moment and assume a force is exerted tangent to the con- 
tact surface. This is simply the case of coulomb friction, and the maximum 
tangent force will be 

Ft = fiF. (6.20) 

In order to further simplify this analysis, assume the maximum moment 
which can be exerted about the contact normal is a linear function of the 
normal force, 

M = fi m F, (6.21) 

where fi m is given in equation 6.18. 

The purpose of this analysis is to determine the contact type as a function 
of the contact wrench. Given the contact wrench in the screw coordinates 



w = 



w 2 
w 3 
W4 

W 6 



(6.22) 



where w 4 and w 5 will automatically be zero, since we are only considering 
fingertip contacts. The relationship between the contact wrench and the 
contact type, given the linearizing assumptions discussed above, will be 



\ J w l + ™l < \f*fw 3 \ and y/w% < \fi m w 3 \ =► 

y jw\ + w\ < \/J, f w 3 \ and 

yjw\ + w\ > \n f w 3 \ 
w 3 >0 



w\ > |^m"> 3 | 



1 
2 
3 

4, 



(6.23) 



where 



1 Soft finger contact 

2 Point contact with friction 

3 Point contact without friction 

4 No contact. 
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Figure 6.3: When the force tangent to the contact exceeds n; times the normal 
force, assume the contact is sliding on the surface. If the tangent force is less than 
H/F, and the moment about the contact normal exceeds fi m times the normal 
force, assume the fingertip is twisting relative to the contact surface. Finally, 
if both the tangent force and moment are less than n/F and fj, m F respectively, 
assume the contact is rigidly fixed to the surface. 



We will approximate fi m = 1.83 x 10~ 3 in and n = 1.0 for the Salisbury robot 
hand grasping an aluminum can. With the relation given in equation 6.23 
and the analysis of the previous chapter, it will now be possible to determine 
the constraint state of a grasped object as a function of the external forces 
on the object and the squeezing force of the hand. 



Chapter 7 
Controlled Slipping 



7.1 Introduction 

So far only individual modeling issues have been discussed, but we have 
not yet addressed how these results can be combined and how they can 
be used to enhance the dexterity of a robot hand. In this chapter, the 
analyses of the preceding chapters will be integrated together and the result 
will be used to predict and affect some controlled slip motions. To illustrate 
these principles, two examples will be considered. The first is a simple two 
dimensional example of two fingers holding a rectangle in a gravitational field. 
This example outlines the analyses of the previous chapters in some detail 
and allows many of the concepts to be graphically illustrated. The second 
example is of a three fingered robot hand grasping a cylinder. This example 
demonstrates how controlled slip manipulation might be implemented on an 
actual robot system. The next chapter describes how these ideas were applied 
to the multifingered Salisbury robot hand. 

7.2 Two dimensional example 

Two fingers grasp a rectangle on opposing surfaces, as shown in figure 7.1. 
In this particular example, the fingers grasp the block at the center of its top 
and bottom sides. The block has a height of 4 cm and a length of 7 cm and is 
held horizontally in a gravitational field. There are a number of coordinate 
systems used in this example, figure 7.2. These are: the reference frame, 
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hand frame, object frame, and contact frames. These coordinate frames are 
used throughout the analysis and are described in appendix A. 




Figure 7.1: Two fingers grasp a two dimensional rectangular block. In this 
example, we are assuming the block only moves in the xy plane and that gravity 
exerts a force on the block in the negative y direction. 



7.2.1 Constraint 

Contact types 



There are three different contact types for fingertip contacts in the plane: a 
point contact with friction, a point contact without friction, and no contact. 
Each contact type allows only a certain set of wrenches to be transmitted 
through the interface. As with the three dimensional case, these wrenches 
may be described by the set of unidirectional unit basis wrenches, which 
span the space of permissible wrenches. For convenience each contact types 
is represented by a number as follows: 

1 Point contact with friction 

2 Point contact without friction 

3 No contact 
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Figure 7.2: There are five coordinate frames shown in this example, the reference 
frame, the hand frame, the object frame, and the contact frames. The block has 
dimensions 4x7 cm and a mass of 0.2 kg. 

Constraint states 

In the example illustrated in figure 7.1, there are only two contacts. With 
three possible contact types at each contact, yielding a total of 3 2 or nine 
constraint states. As with the three dimensional case, the constraint state is 
described as an ordered list with as many elements as there are contacts. Let 
the contact made by the finger on the top of the rectangle be first element 
in the list and the finger on the bottom be second. Thus, if the fingertip 
on the top of the block were beginning to slip, but the finger on the bottom 
remained fixed, the constraint state of the grasped rectangle would be [2, 1]. 

Permissible motions 



For each constraint state, there are some motions which the object may 
undergo and some which it may not. As before, the set of allowed motions 
can be represented by a set of twists. For consistency, the twist will still be 
represented by the six element vector t = [ti, t 2 , t 3 , t 4 , * 5 , i 6 ]; however, for the 
two dimensional case, the elements t\, t 2 , and t 6 are zero, the elements t 4 
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and t 5 are the translations in the x and y directions, and t 3 is the rotation 
about the normal to the plane. All the elements of the twist are defined with 
respect to some reference frame OXYZ, which in this case coincides with 
the object frame, figure 7.1. 

In the two dimensional case, the set of permissible twists may be rep- 
resented graphically. The direction of motion can be described by a vector 
in three space, coordinate system. Figure 7.3 shows a coordinate system 
with axes labeled t 4 , < 5 , and t 3 , corresponding to the translations in the x 
and y directions and the rotation respectively. The direction of motion may 
be represented by a unit vector radiating from the origin, thus set of twists 
correspond to a collection of unit vectors, figure 7.4. If the twists all lie in a 
plane the set is represented as a section of a shaded disk, otherwise sets of 
twists are shown as sections of a sphere, figures 7.5 and 7.6. 




Figure 7.3: The direction of motion in may be represented using a simple graphic 
technique. Unit twists corresponding to the direction of motion are illustrated as 
a unit vector radiating from the origin of a coordinate system with axes labeled 
U and t s (translations in the x and y directions), and t 3 (the rotations about the 
normal to the plane). 

In the two dimensional case as in the three, the set of permissible motions 
is the intersection of all the unit twists repelling to the unit basis wrenches 



ftm rnm^tm' **»m m**mm><* J.* > <mmtV * A^J^n ^^. jywsB^^^ m m Mmum.mm m m .. I U I) MM 
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Figure 7.6: Unit twists correspond to subsets of the sphere. 

and unit twists reciprocal to the normal basis wrenches which do not violate 
the geometric constraints of the contact surface. For this example, the set 
of unit twists repelling or reciprocal to all the unit basis wrenches is shown 
in figure 7.7. Figure 7.8 shows the additional constraint imposed by the 
geometry of the contact surface. In the diagrams, permissible motion is 
represented by either single vectors, shaded disks and sections of spheres. 

7.2.2 External wrench 

A wrench in two dimensions may be represented by the six element vector 
w = [w u w 2 , w 3 , w 4 ,w 5 , w 9 ], where w 3 , w 4 , and w 5 are zero. The elements w x 
and w 2 represent force in the x and y directions respectively and w 6 represents 
the moment about the normal to the plane. 

Gravity 

The gravitational force on the object can be used to move the object within 
the grasp. Assume the rectangle is in a gravitational field, with an acceler- 
ation g in the negative y direction. Let the rectangle have a mass m. Then 



«msm**!<*«~*~>^>'' i . >"' "" u' m. u j**?^, .! ^^ H ^^ , , i ,,i IL .in . 1 
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Figure 7.8: Constraint sUtot far the 
geometry 



rectangular block including local surface 
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wrench on the object in screw coordinates defined with respect to the OXYZ 
reference frame is 





w 



-mg 







(7.1) 



and with respect to the object frame, it is 



w = 



— m<7sin0 


—mg cos 6 















(7.2) 



Suppose we can reorient the block in the xy plane to any angle 9, figure 
7.9. Through the use of gravity, we have means of changing the wrench on 
the rectangle with respect to the object frame. If the block have a mass of 
0.2039 kg, the external wrench (in newtons) is 



w = 



" -2sin0 ' 


-2cos0 















(7.3) 



7.2.3 Grasping force 

Since there are two point contacts between the hand and the object, the 
fingers can exert an arbitrary squeezing force. Defined relative to the contact 
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Figure 7.9: The rectangle can be reoriented in a gravitational field to any orien- 
tation 



frames at each of the fingers, this internal force will be 





-F/2 


wi = w 2 = 






(7.4) 



7.2.4 Contact wrenches 

Since we are only considering point contacts, in this planar example, only 
translational stiffness is possible, as shown in figure 7.2.4. The stiffness nor- 
mal and tangent to the contact surface are given by 

Kormal = 5N/cm 

kangent = 2N/cm. ( 7<5 ) 

The wrench at the i th contact defined with respect to the contact frame is 
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wj = k c .Ti 'K^w, + wgi, 



(7.6) 



where, 



1. k Ci is 6 x 6 element stiffness matrix representing the the stiffness at the 
contact point in terms of the contact frame, 



k, ! = 



Note the values of the stiffness are negative, since we are interested in 
the force on the object as a result of the displacement of the fingertips. 
Replaced k Cl and k C2 with the numerical values 
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2. Ti is a transformation matrix for the i th contact denned in chapter 5. 
For two dimensions, this is 



Ti = 







l x m x ' 
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and the inverse 
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Ti = 



mj = [m x ,my]i and nj = [n x ,n y ]i are the tangent and normal vectors; 
and, Xi and j/ t is the location of the i th contact. Therefore, in this 
example, 
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3. K is the total stiffness of the grasped object 



t=i 



where lq is the stiffness resulting from the i th contact at the origin of 
the object frame, 

K., — A. ,K C . X • , 

where Tj and Tj" are given above and 



ki = 



and 



k 2 = 
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4. w« is the external wrench, 



w. 



5. Wgj is the internal wrench, 



-2»in0 
-2co»0 













-#72 










Substituting the values far this particular example, the contact wrenches are 



wi 



and 



w, 



-§in# 
-ccef-F/2 





sinl 
cos* - F/2 







(7.7) 



(7.8) 



Plots of the contact wrenches as a function of the squeeze force, F, and the 
orientation $ are given in figure 7.11. 
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Figure 7.11: The normal force and the tangent force at each contact, relative to 
the contact frame, as a function of the squeezing force F and the orientation of 
the block 0. 
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7.2.5 Contact wrench / Contact type relation 

The relation between the contact wrench and the contact type for two di- 
mensions is given in chapter 6, 

| u>i| < \ftw 2 \ =► 1 (point contact with friction) 
kil > \fiw 2 \ =► 2 (point contact without friction) 
w 2 > =*- 3 (no contact). 

Instead of using the absolute values, it the above relations, it would be more 
convenient to determine an analytic function. Let 5,- be a slipping criteria 
defined by 

Therefore the relation between the contact wrench and the contact type is 
Si> 1 => 1 (point contact with friction) 
Si < 1 =>• 2 (point contact without friction) 
w 2i > 1 =>■ 3 (no contact). 



The plots for 5 X and S 2 are shown in figure 7.12, for values of grasping force 
ranging from 2N to ION. S f will approach infinity in some orientations, as 
the grasping force is reduced to zero. Also, as might be expected, the slipping 
criteria will tend toward zero as the grasping force is increased to infinity, 
independent of the orientation of the object. Whenever the rectangle is held 
in a horizontal position, the slipping criteria 5 a and S 2 are both zero, since 
this orientation relies on mechanical rather than frictional constraints. The 
mapping of S, can therefore be used as a measure of stable grasps. The 
function Si will be a minimum on each contact when the ratio of frictional 
force to normal force is a minimum; that is, Si will be a minimum when 
the grasp depends most on geometric rather than frictional constraint. The 
maximum values of 5 X are at tt/4 and 7tt/4 and the maximum values S 2 are 
at 37r/4 and 57r/4. Intuitively, this is because the block's weight is supported 
more by the finger on the bottom, unweighting the top finger. Since the 
tangent force on both fingers is the same, the ratio S { , of the top finger, is 
a maximum. The rectangle is still constrained, since the geometry of the 
surface does not allow rotation. If the surfaces were convex, however, the 
object might drop out. In the next section regions of constraint will be 
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defined as a function of object orientation and grasping force. 

7.2.6 Constraint state map 

A useful plot, in terms of controlled slip analysis, is a constraint state map, 
which defines the constraint state as a function of the controllable variables, 
such as squeeze force, orientation, and externally applied forces. In this par- 
ticular example, the constraint states are a function of the block orientation, 
0, and squeeze force m g , figure 7.13 

Suppose the rectangle is held at a 30° angle relative to the horizontal with 
a squeeze force of 4 N, represented as a point on the constraint state map, as 
shown in the figure 7.13. By varying the orientation and the grasping force, 
it is possible to move about the constraint state map, arbitrarily entering 
and exiting different constraint regions to either avoid unstable grasps or 
to produce the desired slipping motions. For example, suppose we want to 
reorient the block by sliding it between the fingers. First, determine which 
constraint states, if any, allowed this motion. As can be seen in the graphs 
in figure 7.8, there are four such constraint states: [22], [23], [32], and [33]. 
Second, from among these constraint states choose the one which produces 
the maximum constraint on the object, thus minimizing inadvertent motion. 
In this case, constraint state [22] is the maximally constrained state which 
allows the desired motion. Third, to produce this constraint, it is necessary 
to move from the present location in region [11] in the constraint state map to 
any point in region [22], as shown in figure 7.13. In the region [22] the block 
may slide through the fingers, but the direction of motion is determined by 
the orientation of the block. More generally, the direction of motion is the 
intersection of the permissible and preferred twist, which in this case yields 
a single vector. 

Consider another example. Suppose two fingers grasp adjacent sides of 
a square, as shown in figure 7.14. Using the assumptions as in the previous 
case, the graphs of permissible twists are shown in figure 7.15 and a map of 
constraint state is produced in figure 7.16. Therefore to ensure a stable grasp, 
it is necessary to remain in the constraint state region [11]. If the grasping 
force is increased the constraint state changes from [11] to [22], which re- 
sults in an unstable grasp. The subsequent slipping motion in this region 
is bounded as shown in figure 7.15, but its exact direction is unpredictable. 
Constraint regions [12] and [21] yield sets of permissible twists which contain 
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Figure 7.12: This is a plot of the slipping criteria for the fingers on the block. 
The plot for finger contact on the top of the block is on the top of the page and 
the plot for the finger on the bottom is on the bottom of the page. When the 
curve is a minimum, the ratio of the tangent and normal force at the contact is a 
minimum. In other words, there is a greater dependence on structural constraint 
rather than frictional constraint at minimums of the Si plots. 
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only a single element; however, these sets of permissible twists do not inter- 
sect the set of preferred twists when the object is moved into these constraint 
regions. 

Using the constraint state map, along with maps of the permissible twists, 
it is possible to analyze and actuate controlled slip motion for grasped ob- 
jects. In the next section, a more general three dimensional example will be 
considered. 



7.3 Three dimensional example 

The three dimensional analysis is essentially the same as is two dimensions. 
The goal is to generate a map of the different constraint states as a function 
of the controllable variables, such as the external wrench, grasping force, 
stiffness, etc. Consider the Salisbury three fingered robot hand grasping a 
can, as shown in figure 7.17. The robot has two fingers on the top of the can 
and a third on the bottom, equally spaced between the top two. The grasp 
is somewhat off center from the centroid of the can. 

Figure 7.18 shows the coordinate frames which will be used the analy- 
sis: the hand frame O h X h Y h Z h , the object frame X Y Z , the grasp frame 
O g X g Y g Z g , and three contact frames 0,X,Y;Z,. Descriptions of these coor- 
dinate frames are given in appendix A. The centroid of the can is given by 
a point in the hand frame Xc g = [-0.8,2.7, -3.0] cm. The can has a diam- 
eter of 2.5 in (6.4 cm) and length of 5.0 in (12.2 cm), and mass 0.386kg. In 
this example, the hand holds the can at a 45° angle in a gravitational field. 
Therefore, the external wrench on the can, in terms of the hand frame, is 



We = 





0.273 
-0.273 

0.082 
-0.218 
-0.218 



N. 



(7.10) 



By changing the orientation of the can or by squeezing it, we can effectively 
control the constraint state. Suppose we only consider changes in the grasp- 
ing force [x g , y g , m g ]. The constraint state map will then be a three dimension 
map, where the constraint states will be a function of x g , y g , and m g . Figure 
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7.19 shows two "slices" for constant grasp force magnitude, m g = 0.7 TV and 
m g = 2.0N. The shaded areas represent regions in which the can is fully 
constrained and will not move. 

Suppose the grasp force focus is given by x g = 0.0cm, y g = -1.2cm, 
and m g = 0.7N, corresponding to a point A within the [2,2,2] region of the 
m g = 0.7N slice of the constraint state map. A reorientation of the can is 
possible, in this example, by allowing it to rotate between to of the fingers. 
Regions [3,2,2] and [4,2,2] allow a single rotation about the second and third 
fingers. Therefore by moving the grasp force focus from the current location 
in the [2,2,2] region to any point in the [3,2,2] region, the can will rotation 
between the fingers as desired. 

As in the two dimensional case, we can choose an arbitrary slipping mo- 
tion and determined the maximally constrained state which allowed the de- 
sired motion. This constraint state can then be produced by adjusting the 
controllable variables such as grasping force and orientation, as indicated 
by the constraint state map. In the next chapter, we will implement this 
procedure on a robot hand. 



CHAPTER 7. CONTROLLED SLIPPING 



78 



m„ 




Figure 7.13: The map of the constraint states as a function of the grasping 
force and the object orientation can be used both to find stable grasps securing 
the object and unstable grasps allowing the object to slip. In this example, the 
current state of the grasped rectangle is shown as point A in the figure. By moving 
along the indicated path, the constraint of the object can be change from [11] to 
[22]. Once in constraint state [22], the rectangle is to slide between the fingers. 
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Figure 7.16: This is a map of the constraint states as a function of the grasping 
force and the orientation of a square held by fingertips on adjacent sides. Notice 
that simply increasing the grasp force does not guarantee a stable grasp. A stable 
grasp is only possible in the shaded [11] region. Any other region in the constraint 
state map will result in an unpredictable slipping motion. 
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Figure 7.17: By controlling the grasping force, the can be held securely or 
reoriented by controlled slip. 




Figure 7.18: There are six coordinate frames used in this example: the hand 
frame, the object frame, the grasp frame, and the contact frames. The hand is 
rotated 45° is a gravity field. 
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Figure 7.19: The constraint state map, shown here, is a function of the grasping 
force, x g ,y g , and m g . Two "slices" of the three dimensional constraint state map 
are shown, for m g = 0.7N and m g = 2.0N. The current position on the constraint 
state map is shown as a point A in the [2,2,2] region, within the m g = 0.7N slice. 
By moving from the present position in the map to any point in the [3,2,2] region 
the can is free to rotate between the fingertips. 



Chapter 8 
Implementation 



8.1 Introduction 

The purpose of controlled slip manipulation is to enhance the dexterity of 
a multifingered robot manipulator, giving a robot hand a larger repertoire 
of manipulation strategies. A number of programs were written to automat- 
ically analyze the constraints on an object within a particular grasp. One 
function accepts a desired motion and returns the maximally constrained 
state which allows that twist. Another determines the constraint state on 
the object as a function of the grasping force. All these functions were writ- 
ten in LISP and were used in conjunction with the Salisbury robot hand. 
The following sections describe the hardware of the robot system and the 
software implementation of the slip analyses. 

8.2 Description of Hardware 

The Salisbury robot hand is shown in figure 8.2. The hand has three fingers 
and three joints in each finger, allowing a total of nine degrees of mechanical 
freedom. Each of the fingers is controlled by four steel tendons running 
to torque motors in the forearm. Linear encoders on the motors and tendon 
tension sensors in the fingers determine the joint position and torque allowing 
the hand to operate in a position, force, or stiffness control mode. 

The general control structure is shown in figure 8.2 Two modified Puma 
robot controllers accomplish the lower level servo control, while higher level 

84 
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Figure 8.1: The Salisbury robot hand has three fingers and three joints in each 
finger, yielding a total of nine degrees of mechanical freedom. The hand has linear 
encoders on the motors to determine joint position and tension sensors on the 
tendons to resolve the joint torque. 

fingertip and joint trajectories are directed from the VAX 11/750. At a still 
higher level, the Symbolics 3600 controls the coordinated hand functions. 

8.3 Description of software 

A program, GRASP, was written to analyze the constraints of an object 
grasped by the three fingered robot hand. The program was designed to 
be interactive, allowing the user to select specific grasp analysis functions, 
control the location and orientation of the grasp, or to choose a particular 
squeeze force. The functions can be selected from a menu, and the results of 
the analyses are displayed on a graphics window, illustrated in figure 8.3. The 
screen is divided into three regions. The large pane to the left is a graphics 
screen showing the hand, finger, object, and contact locations. The smaller 
screen on the top right is LISP Listener, which accepts and interprets LISP 
commands. Finally, on the bottom right, a small message monitor screen 
prints messages which are sent and received from the VAX. The floating 
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Figure 8.2: The robot hand three basic levels of control. At the lowest level 
PUMA robot controllers implements the high speed servo control, at the inter- 
mediate level a VAX 11/750 controls the joint trajectory and finger stiffness, and 
Finally, at the highest level a Symbolics 3600 LISP Machine manages the coordi- 
nated hand motion. 
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menu allows the user to select many graphic and actuation options. Each 
of the options will be described briefly in the following sections. Functions 
which employed the controlled slip analyses will be described in more detail. 

8.3.1 GRASP functions 

Standard Graphics Options: 

CREATE GRASP SCREEN. Creates the three paneled display screen 

CLEARSCREEN. Clears the graphics screen 

DRAW COORDINATE SYSTEM. Draws the six coordinate systems on the 
graphics screen. These are the hand frame, object frame, grasp frame, 
and the three contact frames. These different coordinate systems are 
fully described in appendix A. 

CHANGE GRAPHICS VARIABLES. Allows the user to change the scaling 
and the graphics screen origin. 

Basic Actuation functions: 

GO HOME. Reinitializes the VAX and Lisp Machine trajectory lists and 
goes to the home position 

REINIT VAX TRAJECTORY. Reinitializes the trajectory list on the VAX 

REINIT OOLAH TRAJECTORY. Reinitializes the trajectory list on the 
LISP machine 

BASIC MOVES. Allows the users to specify coordinated finger motion. The 
options allow the user to translate or rotate the grasp frame in any 
desired cartesian direction. 

MOVE FINGER. Moves a selected finger by a specified displacement 
Controlled slip analyses: 

CHANGE GLOBAL VARIABLES. Allows any of the global variables to be 
changed. 
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Figure 8.3: The GRASP program was written as an interactive interface between 
the user and the Salisbury robot hand system. The program allows the user 
to select a number of functions, including: slip analysis functions, functions to 
relocate and reorient the grasp, and a function which changes the grasp force on 
the object. 
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PERMISSIBLE TWIST. Specify a desired twist and this function will return 
the maximally constrained state of the object that allows that twist. 
This is accomplished simply by looking at each contact individually 
and determining which contact type will allow the specified twist. The 
contact types are investigated in decreasing order of constraint. That 
is, a soft finger contact, a point contact with friction, a point contact 
without friction, and no contact were considered sequentially. When a 
particular contact type allows the specified motion, this contact type is 
stored. The contact types are then compiled into a list, thus yielding 
the constraint state which allows the desired motion. This constraint 
state is also maximal, since any state of greater constraint would not 
allow the twist to occur at one or more of the contacts. 

DETERMINE CONSTRAINT STATE. This option allows the user to spec- 
ify the grasping force, [x g ,y g ,m g ], and returns the constraint state of 
the object for a specific orientation and a given grasp. The program 
uses the stiffness model described in chapter 5 together with the contact 
type/contact wrench relation given in chapter 6. 

MAP CONSTRAINT SPACE. The user specifies the magnitude of the grasp- 
ing force, m g , and the program produces a map of the constraint states, 
by varying the grasp force focus, [x g ,y g ]. The program plots the re- 
gions of different constraint and prints the value of the constraint state 
in each region. For example, figure 8.4 shows a slice of the constraint 
state map for m g = 7.0N for the same situation described in chapter 
7. Notice that the coordinate systems of the hand, the grasp frame, 
and contact frames are drawn in the display, since the grasp force focus 
corresponds to an actual point in space. From this point the user can 
select the grasping force [x g , y g , m g ] by using the option PICK GRASP 
FORCE CENTER. 

Advanced Hand Actuation Options: 

MOVE TO CONTACT POINTS. Moves the fingertips to specified contact 
points on the object. This function takes into account the radius of the 
fingertip when grasping an object 

PICK GRASP FORCE CENTER. The user specifies a grasp force magni- 
tude m g . Then the program draws the grasp frame coordinate system 
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on the screen, along with the hand, and contact coordinate systems. 
The user may then move to any point in the graphics screen and select 
a grasp force focus, [x g ,y g ], on the grasp plane. The robot will then 
displace the fingers by an amount and a direction proportional to the 
specified grasp force, [x a ,y g ,m g ]. Given the compliance of the fingers, 
the resulting force will be proportional to the desired internal grasping 
force at each of the fingers. 

DEMONSTRATIONS. Three demonstration routines were written to illus- 
trated how controlled slipping techniques might be used on an actual 
robot hand. The three routines show respectively how gravity, free fin- 
gers, or other objects may be used to reorient and reposition an object 
within a grasp. 

The first demonstration is the three dimensional example of chapter 
7. The robot hand holds a full can of liquid, as shown in figure 8.5. 
The mass, location, orientation, and surface properties of the can are 
the same as those given in chapter 7. A constraint state map shown 
in figure 8.6, for a constant grasp force magnitude of 0.7N. Initially, 
a grasp force, [x g ,y g ,m g ] = [0.0cm., -1.2cm., 0.7 N] is selected so that 
the can is in the [2,2,2], a fully constrained region of the constraint 
state map. The magnitude of the grasp force remains the same, but 
the focus is moved out of the [2,2,2] region into the adjacent [3,2,2] 
region. The result, shown in figure 8.7, is that the can spins between 
the two fingers, under the influence of gravity, into a new orientation. 
The grasp force focus is then moved back into the [2,2,2] region and 
the grasp is again secured; however, the can has rotated 90°. 

In the second demonstration, the robot hand again holds a can in the 
same position and orientation. One of the fingers is then removed from 
the top of the can. The situation is essentially a two fingered grasp or 
a three fingered grasp with the first finger in a type 4 contact. In any 
case, the magnitude of the squeezing force between the two grasping 
fingers can be controlled. In this case, the magnitude of the squeezing 
force is relaxed, so that when the free finger exerts a force on the front 
of the can, the constraint moves into the [2,4,2] constraint region and 
spins between the two fingers. The finger continues to spin the can, 
until it has been rotated 180° into a new orientation, figure 8.8. The 
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free finger is then replaced on toe can, securing the grasp. The can has 
been completely reoriented in the graap. 

In the third routine, the hand grasps a box and pushes it against a table, 
so that the box slides through the fte*ers. The box k then lifted from 
the table and the free finger spins it 109*. The hand again grasps the 
box and again forces it against the table, repenting the same procedure 
over and over again, as shown in figure 9.9, 



r~\ 
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Figure 8.4: Constraint state map produced by the computer plots the constraint 
state as a function grasp force focus x g y g for constant grasp force magnitude, 
m g = 2.0N. The map show here is for the identical situation outlined in chapter 

7. 
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Figure 8.5: The Salisbury robot hand holds a can, as in the example in the 
previous chapter. By adjusting the squeezing force on the can, the robot can 
either secure the can in the grasp or change the orientation by allowing it to slip. 
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Figure 8.6: This is a map shows the constraint state regions as a function of the 
grasping force focus x g y g for constant grasp force magnitude m g = 0.7 TV. The 
current position in the map is shown as a point in the grasp plane. The current 
constraint state is [2,2,2], a fully constrained region in the map. However, by 
moving to any point in the adjacent [3,2,2] region, the can will spin between the 
fingers. 
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Figure 8.7: The Salisbury robot hand grasps a soda can. The magnitude of the 
grasping force is 0.7 N and the grasp force focus x g = 0.0cm y g = -0.8cm. so 
that the can is secure in constraint region [2,2,2]. However, by moving the grasp 
force focus to x g = 0.8cm y g = -0.8cm, the can will spin between two fingers. 
The grasp force focus is then moved back into its original position and the grasp 
is again secured. 
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Figure 8.8: A can is initially held securely in the robot hand. The grasp force 
focus shifts from its current stable position to a point on the line intersecting the 
contact points made by the first and third fingers. By removing the second finger, 
a constraint state [1,4,1] is produced and by using the free finger to exert a force, 
the can rotates in the grasp. In this demonstration the finger continues to spin 
the can until it has rotated it 180°. Finally, the free finger is returned to its origin 
position and the grasp force focus is moved to the center of the grasp. 
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Figure 8.9: Slipping motions can be linked to form a sequence of motions to 
reorient a part within the hand. In this case, the hand pushes a box against the 
table, forcing the box through the grasp. The hand then lifts the box, reduces 
the grasping force magnitude, shifts grasp force focus between two of the fingers, 
and uses the free finger to spin the box 180°. The hand again grasps the box and 
forces it against the table and then repeats the entire process. 
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9.1 Introduction 

The goal of this thesis, is to gain a basic understanding of a highly dex- 
terous type of manipulation that we take for granted, namely controlled 
slip manipulation. An analysis was performed on grasped objects to 
determine which ways they can move in the grasp. The set of possible 
motions was found as a function of the constraint state of the object, 
that is, the number, location, orientation, and the type of each con- 
tact. The constraint state is a function of a number of controllable 
variables, such as the grasp force, orientation, and stiffness of the fin- 
gers. By changing the grasp force, for example, the constraint state of 
the object can be varied to allow slipping within the grasp. 

This analysis is only a beginning and many important issues have yet 
to be addressed. This chapter will outline some extensions to the gen- 
eral problem of controlled slip manipulation. It is hoped that these 
extensions will allow this type of manipulation to be more useful and 
predictable, thus enhancing the dexterity of robot hands. 
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9.2 Determining permissible motion 

When a hand grasps an object, there are only so many ways that it can 
move in a grasp. For some constraint state, the space of permissible 
motions of the object is quite large, which makes the control of the 
motion difficult. However, there are some constraint states which yield 
a very limited range of permissible motions, so that the motion of the 
object can be determined as a function of the geometric constraints 
alone. With the current algorithm, a desired twist is specified and the 
constraint state which allows this twist is determined. Although the 
algorithm determines the constraint state which provides maximum 
constraint while still permitting the desired motion, it does not reason 
as to which motions would be more advantageous for a given grasps or 
particular task. What would be more useful in terms of controlled slip 
manipulation, would be to determine, perhaps, using a set of heuris- 
tics, a small set of permissible motions, which may be easily achieved 
through slipping motion. To accomplish this, using the current analy- 
sis, would require searching a six dimensional space to determine the 
extent to which the permissible twist is bounded. This may in fact not 
be too difficult, but a set of rules may quickly and efficiently produce 
a set of slipping motions the robot can accomplish. This approach is 
being investigated and looks promising for practical implementation on 
a robot hand. 

9.3 Determining constraint state 

In tandem with the problem of determining set of slipping motions 
which can be easily implemented in a particular grasp, is the problem 
of finding the desired constraint. With the present analysis, a con- 
straint state map is generated as a function of controllable variables on 
the object. To find a desire constraint state, the entire map is generated 
or the space is searched until the values of grasp force are found which 
produce the desired constraint state. Again, a set of heuristic rules may 
allow the robot to quickly find a particular constraint state within a 
large space, so that the slipping manipulation may be achieved quickly. 
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These rules may be quite simple, since there are some constraint states 
which are independent of some of the controllable variables. For exam- 
ple, some grasp foci always produce a particular constraint state. 



9.4 Global motion 

The current analysis only determines the set of permissible twists for 
small finite motions. In general, to reorient an object within a hand, 
large motions need to be considered. In order to study global motions, 
many other issues must be addressed. For example, the size, shape, 
location, and orientation of the grasp object, the robot hand, and the 
surrounding environment must be known. By modeling the robot, the 
object, and the environment as polyhedral solids the global motion of 
the object may be determined. This problem is not as difficult as the 
general path planning problem, since the path is given as the particular 
twist, only magnitude of the twist needs to be determined. Therefore 
for a small set of desired twists, their magnitudes can be found and 
with this information controlled slip trajectories may be planned and 
implemented on a robot hand. 



9.5 Sensory feedback 

The discussion so far deals with slipping motion based on a model of 
the robot, object, and environment. The robot has no feedback on ac- 
tual object position or contact force. In order to performed controlled 
slip manipulations accurately, the robot must know the position of the 
object relative to the grasp, as well as, the location, orientation, and 
type of contact which exists at each interface between the manipulator 
and the object. The contact information may be determined using a 
force and tactile sensing surfaces on the fingertips of the robot hand. A 
contact resolving sensor was built for the Salisbury robot hand, which 
determines the location of a contact on the surface, as well as, the 
normal and tangent force at the interface. Appendix C describes this 
sensor and issues involved in its design. A sensor like this is important 
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in controlled slip manipulation, because it can determine the location 
of the contacts and, using a model of the fingertips, determine the 
contact type. However, this type of sensor may be used directly to 
determine contact type. Since slipping usually coincides with high fre- 
quency vibrational noise, slipping may be determined by looking at the 
high frequency components of the force signal. Given a high frequency 
force signal, together with the mean tangent and normal force readings, 
it may be possible to determine whether the slipping was translational 
or rotational. That is, given a high frequency vibration which suggests 
slip, then 

kc„//"0c„| « 1 =► C 3 

\ w c x /(lWc y \ < 1 =► C 2 

where Ci is a point contact with friction, the object is rotating about 
the contact point, and C$ is a point contact without friction, the object 
is translating at the contact. 

Determining the location of the object in the grasp is somewhat more 
difficult. For some class of objects and for some set of contacts, the 
tactile data alone may be enough to uniquely determine the position 
of the object. However, tactile sensing alone would not, for example, 
resolve the location of a cylinder held vertically on its sides by the fin- 
gertips of a hand. The contacts and contact forces in this situation are 
independent of the vertical position of the cylinder. Some other means 
than tactile sensing would be necessary to determine position. Vision 
would be a possible solution, except that resolving object location at 
the speeds necessary for controlling slipping motion is currently an im- 
possible task. However, vision would seem to be the eventual solution, 
since in order to manipulate objects intelligently, a robot would have 
to know not only the position of objects within the grasp, but also the 
position of objects in the environment, since they may interact with 
the motion of the grasped object. A robot could both avoid external 
objects and use them to exert controlled forces on the object within 
the grasp. 
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9.6 Integrating manipulation techniques 

The purpose of a robot is to interact with the environment, to sense its 
nature and to affect changes. To this end, higher lever plans are needed 
to determine global actions, as well as, lower level strategies which 
integrate manipulation techniques, including controlled slip. Methods 
must be developed to select manipulation strategies to accomplish the 
lower level goals as set by the higher level plans. For example, to invert 
a cylinder within a grasp, a robot may put the object down, reposition 
the hand, and regrasp the object, effectively inverting the can in the 
grasp. Or, it may, using control slip, spin the cylinder between two 
of its fingers, and thereby accomplishing the same feat. In spinning 
the cylinder, the robot could use gravity, acceleration, other objects, or 
other fingers to reposition the object. To achieve the desired goal state, 
the robot may have to perform a number of slipping manipulations in 
a row. This suggests some sort of lower level slip planning based on 
analysis or experience. 
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9.7 Conclusion 

This thesis presents a basic analysis of controlled slip manipulation and 
suggests some methods of implementing slip manipulation on robot 
hands. Given a particular grasp, the set of small finite motions the 
object can undergo within the grasp was determined. The set of per- 
missible motions within a grasp was found to depend on the constraint 
state, that is the location and the types of contacts that exist between 
the robot and the object. The constraint state in turn depends a num- 
ber of controllable variables, such as grasping force, orientation, finger 
stiffness, and other variables. By controlling the constraint state and 
external forces which act on the grasped object, controlled slipping 
motions can be achieved. Before this type of manipulation can be 
practically implemented in robot system, more efficient ways must be 
found to analyze grasps and actuate motions. This thesis does, how- 
ever, demonstrate that manipulating objects by allowing them to slip 
and twist at the fingertips is possible for robots as well as for humans. 
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Appendix A 



Coordinate frames 



A.l Introduction 

Objects, fingers, fingertips, and contact points can all described relative 
to different coordinate frames. There are a number of coordinate frames 
used in this thesis and any point in space may be described with respect 
to any or all the coordinate frames. There are a total of nineteen 
reference frames. These are: 

OXYZ Arbitrary reference frame 

OhXhYhZh Hand frame 
°Sij X hi Y JiJ Z hi Finger frames 

O u X u Y u Z u Fingertip frames 

Ci X Ci Y Ci Z Ci Contact frames 

OgX g YgZg Grasp frame 

X Y Z Object frame 

Descriptions of the individual reference frames are given in the following 
sections. 
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Figure A.l: Hand frame 

A. 2 Hand frame 

The origin of the hand frame is centered between the joints of the first 
and second fingers of the Salisbury hand. The x-axis is directed out 
from the origin, through the center of the first joint of the second finger, 
as shown in figure A.l. The z-axis is vertical, normal to the plane of 
motion of the first phalange of the first finger. The y-axis is normal to 
both the x and y axes, directed away from the wrist, lying in the plane 
of the motion of the first phalange of the first finger. 



A. 3 Finger frames 

The coordinate frames for the finger phalanges are defined the same way 
each finger. These coordinate frames are designated Oj^Xf^Yf^Zfj, 
where i represents the i th finger and ; the j th phalange. The finger 
frames are shown in figure A.2. The origin of the coordinate frames for 
the phalanges are all at the center of the joints and the y-axes of every 
frame is aligned with the central axis of the phalange. The x and z 
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Figure A.2: Finger frames 

axes, however, are denned differently. For the first phalange, the z-axis 
is the axis of joint rotation and the x-axis is normal to the y and z axes 
in the direction of the x-axis of the hand frame. For the second and 
third finger phalange frames, the x-axis is the axis of joint rotation and 
the z-axis is vertical, parallel to the z-axis of the hand frame when the 
joints are in the zero position. 

The length of the phalanges the fingers of the Salisbury robot hand are 
all the same. Figure A. 3 shows a schematic of the hand illustrating the 
length of the phalanges and the placement of the fingers 



A. 4 Fingertip frame 

The fingertip frames O ti X t . Y u Z ti are defined the same way for each 
fingertip, see figure A.4. The origin of the fingertip frame is the center 
of the spherical portion of the fingertip. The z-axis is the central axis of 
the fingertip. The x-axis is parallel to the x-axis of the distal phalange 
coordinate frame. 
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Figure A. 3: Phalange length and finger placement 
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Figure A.4: Fingertip frame 
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Figure A.5: Contact frame 

A. 5 Contact frames 

The contact frames are defined in terms of the object the hand is grasp- 
ing. The origin of the contact frame is the contact point between the 
hand and the object. The z-axis is the surface normal at the contact 
point. The x and y axes lie in the tangent plane of the surface. For 
convenience, the x-axis is defined to be parallel to the x-axis of the 
hand frame or to lie parallel to the plane described by the x and y axes 
of the hand frame, figure A.5. In this way it is possible to generate an 
unambiguous definition of the contact frames. 



A. 6 Grasp frame 

The grasp frame is defined in terms of the contact points. The origin 
of the grasp frame is the centroid between the contact points. The z- 
axis is normal to the plane described by the three contact points. The 
x-axis is defined in the same way as the contact frames. That is, the 
x-axis is either parallel to the x-axis of the hand frame or parallel to 
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Figure A.6: Grasp frame 
the plane described by the x and y axes, figure A.6. 



A. 7 Object frame 

The origin of the object frame is the located at the centroid of the 
object. The axes of the object frame are arbitrary, but are usually 
defined along the symmetries of the object for convenience. 



Appendix B 

Stress state in a fingertip 



B.l Introduction 

A recent entry into the field of robotics has been the development 
of robot hands. These hands typically have at three or more fingers 
with three or more joints in each finger. The fingertips are usually 
covered with some kind of compliant material. The Salisbury robot 
hand had hemispherical polyurethane fingertips, similar to that found 
on rollerskate wheels. Although most work has be done investigating 
grasping and manipulation of objects, little has been done in analyzing 
the complex mechanical interaction between the finger and the grasped 
object. As a first attempt in addressing this problem, I will examine 
the stress throughout the body of the fingertip in contact with a flat 
object. 



B.2 Problem definition 

The robot fingertip is shown schematically in figure B.l, touching a flat 
surface. The problem will be to find the stress throughout the body 
of the fingertip. The fingertip is assumed to a hemisphere made of 
a homogeneous elastic material. The material on the real robot is a 
polyurethane with an elastic modulus of approximately 40,000 psi and 
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Figure B.l: Robot finger in contact with a flat surface 

a Poisson's ratio of about 0.45. For simplicity in the analysis, however, 
particularly in the finite element procedure, the poisson's ratio will be 
assumed to be zero. Since the elastic modulus is small, as it is for 
most elastomers, a nonlinear analysis would seem necessary; however, 
for this analysis I will assume the contact force is small 1 lb. This force 
should be small enough so that displacements within the body will not 
large enough to warrant a complex nonlinear analysis. 

B.3 Analytic solution 

The analytic solution to the stresses throughout a body in contact will 
another body has not been worked out. The analytic solution for the 
stresses on the surface of a body, however, has be developed by Hertz. 
He assumed the two bodies were solid elliptic disks, each possessing 
radius of R and R'. Furthermore, these disks were in contact along a 
common axis under an applied load F, figure B.2. Hertz deduced that 
the pressure distribution between the two bodies can be described by 
a semi-ellipsoid of pressure constructed over the surface of the contact, 
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Figure B.2: Two general bodies in contact 

as shown in figure B.3 

The pressure distribution is given by 

p{*> y) = Po\Ji - xy a * - J/V6 2 (b.i) 

The total load, therefore, is equal to the volume of the semi-ellipsoid, 

2nabP 

~ (B.2) 

(B.3) 

(B.4) 
(B.5) 



Solving for P 



F = 



P = 



3 

3F 

2-Kab 



where a and b are given by Timosheko [Lipson] 



a = ma 



3FA 



b = n\ 



I 4(A + B) 

I 3FA 

i(A + B) 
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P(x,y) 




+ Y 



Figure B.3: Pressure distribution in the area of the contact 

where 

A = (1 - f il)/E 1 + (1 - pD/Et (B.6) 

where 0.15 

Hi/j,? Poisson's ratio 
E x E-i Modulii of elasticity 

* + **(*■+*+*+*) 

B ~ A ty(* " *) + (it - *)" + 2 (* - *)' (i - k) ««(*) 



t/> The angle between the planes contacting the curvatures 1//J, and 

1/^2 
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R1R1R2R2 Minimum and maximum radii of curvature of the ellip- 
soid disks at the point of contact 

m, n Constants depending on B — A/B + A 

In the case of the contact between a hemispherical fingertip and a flat 
rigid plate, equations B.4 and B.5 become 

a = b = $3FRxA/4 (B.7) 

and the maximum pressure is given by 



n=0 - 578 te < B - 8 > 

again the definition of A is 

A = (1 -/*?)/£?! + (l-^)/£ 2 (B.9) 

If the flat plate is assumed to be rigid (i.e. E 2 > Ex) then A can be 
approximated as 

A = (1 - p\)/E x (B.10) 

For this problem, 

F = lib 

E x = 40,000psi 

Rx = 0.5in 

n = 0.0 



and therefore 



A = 2.0 x 10 _s 
P = 1247.9^?lb/in a 
a = 0.020 v^Fin 
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Figure B.4: Stress gradients within a sphere due to contact with a rigid plate 



Although Hertz only solved for the stress in the are of the contact, H.R. 
Thomas and V.A. Hoersch computed the stress within the body along 
the loaded axis. 



2a(l/R 1 + l/R 2 ) 

<? xx = <ryy = — — — '— 1 -+- 

7rA 



(l + ^)((z/a)cor 1 (z/a)-l) + 



<7« = 



-2a(l/JZ 1 + 1/R 2 ) ( a 2 

7rA 



2(a 2 + z 2 ) 
(B.ll) 



a' + z' 1 



(B.12) 



Also, because of symmetry, a xx ,ayy, and <r« are the principle stresses, 
and therefore, 

Tyz = T yx = (B.13) 

A plot of the stress gradients due to the contact between a flat rigid 
plate and a spherical fingertip on the loaded axis is shown in figure B.3 
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Figure B.5: Initial finite element mesh 

B.4 Finite element solution 



The finite element mesh that was used to analyze the stress is shown 
in figure B.4. The figure shows the finite element representation of one 
radian "slice" of the hemispherical fingertip. Axisymmetric elements 
are used since the body is symmetric around the axis of the contact. 
Eight node elements are also used, since they more accurately model 
the curvature of the sphere. The contact, however, is more difficult to 
model. A contact element could be used, in which more of the element 
is subjected to a force as the element deflects. However, since the radius 
of contact area is small, 0.019 in, compared to the elements which are 
0.050 in on a side, a close approximation is a single concentrated load 
located at the center of the contact area. A test of this assumption 
and the other approximations will be whether the stresses calculated 
by the finite element solution match those of the analytic. 

To test the validity of the mesh, the stresses between identical nodes on 
adjacent elements are compared. Figure B.4 shows the finite element 
mesh. The vertical line drawn on the figure was the line on which 
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Figure B.6: Line on which nodal stress are compared 

stresses were calculated. Figure B.4, B.4, and B.4 shows the stress on 
the line calculated at nodes on adjacent elements. The two curves in 
each figure represent the nodal stress on the two columns of elements 
(i.e. elements 1 through 10 and elements 11 through 20). In every 
figure, the stresses calculated at the same nodes for adjacent elements 
are inconsistent for values of z less than 0.25 inches. For example, 
figure B.4 shows the stress in the y direction, a yy . At z=0.075 in the 
stress calculated at the same node vary from -20 psi in element 12 to 
-100 psi in element 2. On the other hand, for values of z greater than 
0.35 in, the variation of stress between elements is negligible, as can be 
seen in figure B.4. It is also interesting to note that the stress jumps 
for <r yy occur between elements in adjacent columns, while for cr zz they 
occur between elements in the same column. For T yz , the stress jumps 
between adjacent columns and adjacent rows. 

It is clear from this analysis, that for an accurate prediction of stress in 
the body, this mesh is too coarse. Variations in stresses calculated at 
the same nodes were greater that 100%, for radial distances of less that 
0.25 in from the contact region. Figure B.4 shows a three dimensional 
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Figure B.7: foconmtcncy in cakukted nodtl stress <r. 
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Figure B.8: Inconsistency in calculated nodal atreM a, 
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Figure B.9: Inconsistency in oOcuktod nodal «tr«M <r f 




Figure B.Hh Location of consistect »tr«M 
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Figure B.ll: Stress profile on hemispherical body 

plot of stress with the y and z coordinates plotted in the plane and 
the stress cr yy plotted on the vertical axis. Here it is easy to see the 
inconsistencies in the stress between elements Therefore, for an accurate 
calculation of the stress within the body of the fingertip, a new more 
refined mesh must be constructed. 



B.4.1 Refined mesh 

To solve the problem of nodal stress inconsistencies, a new refined mesh 
is constructed. Figure B.4.1 shows the region in which the mesh need 
to be finer. In figure B.4.1, a new refined mesh is constructed in the 
specified region and figure B.4.1 shows an expand view of that region. 
As before, to test the validity of this new mesh, I compared the stress 
along a single line, as shown in figure B.4.1. The stress calculated at the 
nodal points between adjacent elements, in this mesh shown almost no 
inconsistencies for values of z greater than 0.02 in, as shown in figures 
B.4.1 and B.4.1. This is almost an order of magnitude improvement 
over the previous case. Figure B.4.1 shows the same three dimensional 
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Figure B.12: Region of mesh refinement 

stress profile. Again, the y and z coordinates are plotted in the plane, 
and the cr yy is plotted on the vertical axis. As before, the plot shows 
stress jumps at small radial distances from the origin of contact. How- 
ever, if these few values of stress are ignored, the resultant stress field 
is continuous along the line. Another test for the accuracy of the finite 
element calculations is to compare the results with an analytic solution. 



B.5 Finite element vs. analytic 

To compare the finite element solution against the analytic solution, 
only the stresses on the z axis can be considered. Figure B.5, B.5, and 
B.5 show the stresses <r yy , <r zz , and a yz respectively as calculated by 
the finite element solution and the analytic solution. The figures show 
an excellent correlation between the analytic and the finite element 
solution, particularly for values of z greater than 0.04 in. Ignoring 
these stress calculations near the origin, a accurate representation of 
the stress within the entire body of the fingertip can be obtained. 
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Figure B.13: Refined mesh 
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Figure B.14: Expanded view of the new refined region 
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Figure B.15: Line on which nodal stresses are compared 




Figure B.16: Consistency in calculated nodal stresses: a, 
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Figure B.17: Consistency in calculated nodal stress: a z 




Figure B.18: Larger values of stress at small radial distance from the contact 
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Figure B.19: Agreement between the finite element and analytic solutions: 
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Figure B.20: Agreement between the finite element and analytic solutions: 
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Figure B.21: Agreement between the finite element and analytic solutions: 
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B.6 Solution 

As a result of the consistency in calculated stress between the ele- 
ment and because of the agreement between the analytic and the finite 
element solution, the modified mesh is assumed to be sufficient for cal- 
culated stresses throughout the body of the hemispherical fingertip. 
Figures B.7, B.7, and B.7 show the stress profiles through the body of 
the hemisphere. From the analysis, these stresses can be taken as an 
accurate representation of the true stress in the fingertip. 



B.7 Conclusion 



In this analysis, a finite element method was developed to accurately 
predict the stresses in the body of a hemispherical fingertip in contact 
with a flat object (for relatively small forces). This information can 
be useful, for example, in determining the placement of subcutaneous 
force and tactile sensors. 
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Figure B.22: Stress profile in the fingertip: a. 
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Figure B.23: Stress profile in the fingertip: a z 
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Figure B.24: Stress profile in the fingertip: a, 
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Appendix C 
Fingertip sensor 



C.l Introduction 

A contact resolving fingertip sensor was developed which determines 
the location of a contact, as well as the direction and magnitude of 
the force at the contact point. The sensor is based on silicon strain 
gauges on steel flexures, which measure the forces and moments at 
the center of the shell. From these force readings, it is possible to 
determine the direction of the force vector acting on the shell of the 
fingertip. Using the moment readings, the location of the force vector 
can be determined. The intersection of the force vector with the outer 
surface of the fingertip yields two points. One point corresponds to a 
pulling on the surface of the fingertip. The other point corresponds 
to a pushing force on the sensor. Assuming the fingertip only pushes 
against surfaces, one of the points is immediately eliminated. This 
leaves a single point, the contact point, through which the force is 
acting. Since the sensor resolves the components of the force, it can 
determine the magnitude and direction of the force at the point contact 
as well. 
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C.2 Theory 

The theories presented in this section were developed by [Salisbury]. 
Suppose a force F = [f x , /„, f z ] is acting through a point x<. = [x c , y c , z c ] 
on a convex surface S. The convex surface S is defined as a smooth 
surface in 3ft 3 , such that for every pair of points p x and p^ in S, the 
line mpx + (1 - m)p2 for m e 9ft intersects 5 in only two points pi and 
P2- Also for any two points px and pi with contact normals n x and n 2 
respectively, the dot products of the vector p\ — p\ and the normals 
n\ and n 2 have different sign. This condition allows the unambiguous 
resolution of the location of the force on the surface S. 

Let OXYZ be some reference frame so that every point in S can be 
defined relative to OXYZ. The wrench in terms of screw coordinates 
defined relative to OXYZ due to the force F acting through the point 
Xc is 



w = 



" w x ' 




w 2 




w 3 
w 4 


= 


™$ 




. w 6 . 





fy 
h 

~ z cfy + Vcfz 
Zcjx ^cjz 
-Vcfx + X e fy . 



(C.l) 



Now we wish to describe the wrench w in terms of the wrench axis, the 
pitch, and the magnitude. The direction of the wrench axis A is 



d A = 



U>2 
W 3 



(C.2) 



and the point on the wrench axis nearest the reference frame is 



Xo = 



X 
Vo 
Zo 



1_ 

m 



w 2 w 6 - w 3 w 5 

W3W4 — WiWq 

_ WIW5 — W4W2 



(C.3) 



where m is the magnitude of the wrench and also the magnitude of the 
force 

(C.4) 



m = \J w\ + wl + wl. 
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Any point along the wrench axis can be described by 

x = Xo + Ad A . (C.5) 

The intersection of the line in equation C.5 with the surface S yields one 
or two points. If the intersection yields a single point then the location 
of the force on the surface is immediately determined. However, if 
the intersection produces two points, the location of the contact can 
still be found. Suppose the two points of intersection are xi and x 2 
with normals n x and n 2 . Take the dot product of the force F with 
the normals n x and n 2 . The point associated with the normal which 
produces a negative value of the dot product is the contact point. 

Therefore it is possible to determine the point of contact Xc from the 
wrench w. If it were possible to measure the wrench w, then the 
location, direction and magnitude of a force exerted through a point 
contact on a surface S can be found. This is precisely the concept 
behind the contact resolving fingertip sensor. 

C.3 Design 

The schematic diagram of the fingertip sensor is shown in figure C.l. 
The fingertip has a hemispherical top and a cylindrical skirt which bolt 
onto a loadcell. The loadcell is in the form of a maltese cross and 
is design to resolve all six components of the wrench, that is, forces 
and moments in three cartesian directions. Small semiconductor strain 
gauges are mounted on the legs of the cross. These gauges measure 
strain which is proportional to the bending moment in the beam which 
is, in turn, proportional to the forces and moments. The next section 
describes some critical issues in the design of the loadcell. 
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20 d urometer 
polyurethane 
molded cover 



Stainless steel 
Hemispherical cover 



Stainless steel 
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ACTUAL SIZE 



17-4 PH stainless steel 
-axis loadcell with 
16 500H semiconductor 
strain gauges 



Right angle 
polyurethane 
molded strain relief 



38 AWG (7/46) S.P.Duraflex 9 
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Polyurethane jacket 
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Polyurethane 
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Subminiature locking 
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Figure C.l: The fingertip sensor can determine the magnitude, the location, 
and the direction of a force applied through a point contact on its surface. Small 
semiconductor strain gauges mounted on small steel flexure are used to resolve the 
forces and moments on the fingertip shell. 
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C.4 Design of loadcell 

C.4.1 Introduction 

The loadcell for the force and tactile sensing fingertip must be able to 
resolve all six components of forces and moments applied to it. In order 
to properly design the loadcell, it is necessary to determine the forces 
and moments under which may act on the loadcell, as well as the state 
of stress in each of its members. The fingertip may be subject to an 
infinite variety of forces. However, instead of examining all the possible 
forces, we will consider only a small set of forces which produce extreme 
values of force and moment on the loadcell. This set of forces is shown 
in figure C.2. In the next sections, the forces and moments on the 
loadcell resulting from the externally applied loads will be determined, 
along with the associated stress states in the flexures of the loadcell. 

The analysis of stress states is only part of the design procedure. The 
design must also take into account the manufacture, assembly, and 
gauging procedures, in order to minimize cost and maximize efficiency. 
From this analysis, the design and dimensions of the loadcell will be 
determined. 

C.4. 2 Fingertip sensor dimensions 

Figure C.3 illustrates and lists the symbolic values for the dimensions 
of the fingertip and the loadcell. These values will be used through- 
out the analysis when calculating stress, strain, and deformation. The 
constrained values are 

R = 0.406in 

H = 0.813in 

U = 0.070in 

L p = O.lOOin 

H x = 0.633in 

and the values yet to be determined are 
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Figure C.2: Forces and moments are generated at the center of the sensor as a 
function of the externally applied load. The set of external loads which produce 
maximum internal forces and moments are listed above. Each of these cases will be 
considered individually and the resulting forces and moments will be calculated, 
as well as the stresses they create on the load bearing members. 
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Figure C.3: The dimensions for the loadcell and the fingertip illustrated above 
will be used through the analysis on the fingertip. From the analysis, optimal 
values for the dimensions listed will be found to maximize fingertip performance 
and durability. 



C.4.3 Mechanical properties 

The loadcell is to be machined from 17-4PH stainless steel. This par- 
ticular type of steel was chosen because its coefficient of thermal expan- 
sion matches that of the semiconductor strain gauges. A similar value 
of thermal expansion is desired since both the gauges and sensor are 
heated during the bonding process and cooled, the same contraction 
rates will not introduce an offset strains in the gauges. 

The mechanical properties of the 17-4PH stainless steel are: 
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E = 28.5 x 10 6 lb/in 2 
G = 10.6 x 10 6 lb/in 2 
<j y =80,000-100,0001b/in 2 
€ y = cry/E = 3157 fj, strain 

The mechanical properties of the silicon strain gauges are: 
Maximum strain = 3000 fi strain 

C.4.4 General cantilever beam problem 

The analysis of the cantilever beam will be used throughout the anal- 
ysis of the loadcell, so the general problem is stated here along with 
the standard equations of load, shear force, bending moment, and de- 
formation. 

A cantilever beam is shown in figure C.4. It is subject to a force F and 
a moment M at its end point. The deflection of the end point is given 
by 6 and the angle from the horizontal at the end point is denoted by 

<f>. 

Bending moment 

The load, the shear force, and the bending moment along the beam are 
given by 

Load: 

q(x) = (-FL - M)(x)- 2 + F(x)_ x + M(x -L). 2 - F(x - L)^ (C.6) 
Shear force: 
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v(x) = (FL + M){x). x - F{x)° - M(x - L). x + F(x - L)° (C.7) 
Bending moment: 
M b (x) = {-FL - M)(x)° + F(x) 1 + M(x - L)° - F(x - L) 1 (C.8) 

Deformation 

The displacement of the end of the beam is given by 

. FL 3 ML 2 

8= 3eJ + 1eT < c - 9 ) 

where / is the area moment of inertia given by 

h 3 w 
12 

and the angle <f> is 

, FL 3 ML 

Stress and strain 

In general, the stress due to a bending moment Mj is 

M b y 



a = 



(C.ll) 



where / is the area moment of inertia and y is the distance from the 
point of zero stress to point at which the stress is measured. In the case 
of the rectangular cantilever beam, the point of zero stress coincides 
with the center of the beam. Therefore, the maximum stress due to 
bending is located on the outer surface of the beam. That is, when 
y = h/2, the stress is 

6M b 

a = -^± (C.12) 



h 2 



w 
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C.4.5 Case 1 

Assumptions 

Assume a force is applied to the fingertip on the top of the hemisphere, 
figure C.5. The force is applied vertically along the major axis of the 
sensor. By symmetry, fingertip shell will be displaced vertically, no 
horizontal motion or rotation will occur. Also by symmetry, the forces 
will by applied equally to each leg of the cross. 

Analysis 

A single leg of the cross is shown in figure C.6. By of symmetry, each 
leg of the cross receives an equal force, F/4, and undergoes an equal 
displacement 6, and angle <f> = 0. That is, given 

_ (F/QLf ML, 

<l> --2Er + Tr = (c - 13) 



where 



then the moment will be 



Therefore the loading is 



wfhi 



M = -^ (C.15) 



q(x) = -FL,/8(x)_ 2 + F/A{x)_ x (C.16) 

v(x) = FLi^x)^ - F/A(x)° (C.17) 

M b (x) = -FL,/8(x)° + F/4(x) x (C.18) 

(C.19) 

The maximum bending moment occurs at x = and M b = -FL/8. 
Therefore, the maximum stress in the beam under this particular load- 
ing is 

ZFL 



&max — 



4wihf 



(C.20) 
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C.4.6 Case 2 

Assumptions 

A horizontal force is applied to the fingertip, in line with the loadcell, 
as shown in figure C.7. The force is evenly distributed on either side 
of the loadcell, as a result no rotation occurs. Since, by symmetry, 
both sides of the loadcell will behave the same way, one half of the 
loadcell will be analyzed and is depicted in figure C.8. It is assumed 
the compressive displacements are negligible, therefore, both the legs 
parallel to the applied force and the phalanges are assumed to be rigid. 
In addition, a cantilever beam model is assumed for both the legs and 
the phalanges. 

Analysis 

The two phalanges and the leg of the cross on each side of the load- 
cell are displaced by the same amount S, figure C.8. The ends of the 
phalanges and the legs do not rotate, therefore <f> = and by equation 
CIO 

M { = -FtLi/2 (C.21) 

Substitute into equation C.9, the displacement is 

6 - wk < C22 > 

and the force 

F '=( Ej f i ) s < c - 23 > 

Since the force F is evenly distributed on each side of the loadcell, the 
sum of the force on the each flexure is F/2, that is 

(C.24) 



(C.25) 





F/2 = F P + F, + F P 


Substitute C.22 into C.24 yields 


F/2 = 


'Ew z p h p Ewftn Ew 3 h p 


{ LI ' Lf ' £J J 
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Solve for 6 

FLfLl 

~ 2E(wfh,L* p + 2w*h p L?) (C ' 26) 

The force and the moment on the leg of the cross is 

/ wfh,L* \ F 

l -\ W fh l Ll + 2wlh p tf)l (C ' 27) 

/ wfh t Ll \ FLi 

Ml - ~ UkL^KLf ) -r ( c - 28 > 

(C.29) 
and the force and moment on the phalanges are 

w^hvL? 



Fp [w^LfWLfkLl) 2 (C - 30) 

_ / w}h,Lf \ FL P 

Mp W p h p Lf + 2w!h l L*) 4 (C - 31) 

(C.32) 

Finally, the maximum stress on the leg of the cross is 

_ ZwjLlLtF 

° l ~ 2(wfh,L* + 2w*h p Lf) (C - 33) 

and the maximum stress on the phalange is 

3w p LfL p F 



cr p = 



2(w* p h p Lf + 2wfh,L*) (C - 34) 



C.4.7 Case 3 

Assumptions 



A force is applied at the base of the fingertip shell in the horizontal 
direction as shown in figure C.9. This particular case produces the 
largest stress on the members of the loadcell. The force applied in this 
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orientation and position will therefore be limiting force in the design 
of the loadcell. It is assumed the loadcell undergoes a rotation and 
deformation as illustrated in figure C.10. The phalanges parallel to the 
applied force are assumed to undergo rigid rotation. The phalanges 
perpendicular to the applied force are subject to a moment, but for 
this analysis these phalanges are also assumed rigid. Therefore the 
entire outer structure of the cross undergoes a rigid rotation <j>. As in 
the previous case, the legs perpendicular to the external force undergo 
a displacement, however, since the major stress components are on the 
legs parallel to the external force, these displacements are assumed to 
be negligible. 

Analysis 

A free body diagram of the cross is shown in figure C.ll. The de- 
formation from the force F is assumed small in comparison with that 
from the moment FH X . The two legs perpendicular to the applied force 
undergo a rotational twist subject to a moment M 2 and the two legs 
parallel to the applied force bend under a moment M x and a force F x . 
By equilibrium, 

FHx = 2F x Li - 2M X - 2M 2 (C.35) 

A diagram of a single leg undergoing bending is shown in figure C.12. 
By geometric compatibility, the displacement equals the length of the 
beam times the angle, that is, assuming angles are small. 

S = L,<j> (C.36) 

Substitute into the equations for displacement and angle and solve for 
the moment M x yields 

M x = -FiL, (C.37) 

and the angle <f> 
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Figure C.13 shows a diagram of a beam under the torsional load, and 
the equation for angular rotation as a function of moment is 



<f> = 



-M 2 Li 
c 2 Ghxw\ 



(C.39) 



for w x < h x and c 2 = 0.170 approx. Equate equations C.38 and C.39 
and solve for the moment M 2 



M 2 = 



_ f!8Lic 2 Gwf 



Fx 



Now substitute equations C.37 and C.40 into C.35 
FHx = 2FxL, - 2(-FxL,) - 2 ( l8L ' c ^ Gw f 
and solve for the force Fx 

Fx = 



(C.40) 



V Eh] 
FHx 



Fx 



4L t + ^S"! 



~EWf 



(C.41) 



(C.42) 



Figure C.ll shows a single leg of the cross under the applied loads. By 
equilibrium the sum of the moments at the root of the leg are zero. 

That is, 

- FxL t + Mx + M' = (C.43) 

and using equation C.37 

M' = 2F 1 L, (C.44) 

and is also the maximum bending moment under which the beam is 
subject. Substituting equation C.42 into the above C.44 yields. 

FH ' Ll (C.«) 



*'*max — 



2L >+ Eh? ' 



Therefore the maximum stress is 



^TJlflJ? — 



SFHxLt 



wihf 



L '+ Eh ' i ' 



(C.46) 
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C.4.8 Case 4 

The fingertip may also be subject to a tangent force as shown in figure 
C.14. This case, however, is identical to case 3, except for the length 
of the moment arm. In this case it is R + H - H x instead of H x and 
the maximum stress is therefore 

<r max = K -r LUJ- (C.47) 



wihf 



Li + M-'g ffi 



~Ehf 



C.4.9 Case 5 

Assumptions 

Figure C.14 shows a force applied to the fingertip, tangent to the sur- 
face. It is assumed this force is on the same plane as the loadcell, 
therefore stress is only induced on the sides of the flexures. In this 
case, the stress due to the force and the stress from the induced mo- 
ment superimpose, so that each case must be considered in order to 
find the maximum stress on the members. First we will determine the 
maximum stress due to the moment FR and then add the maximum 
stress determined from case 2. Figure C.15 shows the loadcell rotated 
as a result of an applied moment M . It is assumed all the phalanges 
are rigid and that the entire outer structure of the loadcell undergoes 
a rotation <f>. 

Analysis 

A single leg of the cross is shown in figure C.ll. It is subject to a force F, 
and a moment Mi applied at its endpoint. By geometric compatibility 
the displacement 

8 = <f>Li (C.48) 

and using equation C.9, the moment can be found in terms of the force 

Mi = FiLi (C.49) 
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By symmetry, each leg of cross is under the identical loading as shown 
in figure C.15. The sum of the moments must be zero, therefore, 

FR = 4FiL, + AFiLt (C.50) 

and 

FR 

F < = sir < c - 51 > 

and 

F R 

Mi = -f (C.52) 

A free body diagram of a single leg is shown in figure C.ll, sum the 
moments at the base of the cantilever 

FR 
M/=-£iI (C.53) 

which is the maximum bending moment on the beam. Substitute into 
the equation for the stress, gives, 

ZFR 

* = 2W (C - 54) 

Now we must also consider the stress from the tangent force 

3w,L 3 p LiF 
ai ~ 2{wfh l L% + 2w p h p Lf ) (C,55) 



The sum, therefore, will be the maximum stress on the beam 



<Tl = 



3R SwtLlLi 



2h t wf 2(wfh,L 3 p + 2w p h p Lf) 



(C.56) 



C.4.10 Case 6 



Suppose the fingertip is subject to tangent force on the outer radius of 
the hemisphere. As in the previous case, we must again consider the 
superposition of two stresses, since the stress due to the induce moment 
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and the vertical force superimpose on one of the legs of the cross. From 
the analysis in the first case the stress was 



&max — 



SFL 



4wihf 
and from case 2, substitute R for Hi yields 

_ ZF(R + H-H i)L, 



wihf 



T . 9Lic 2 Gwf 
L '+ Eh ' f ' 



The maximum stress in this case is therefore 



™ rnn t — 



4w ' h ' + w,hf 



ZRLi 



Li + 9L 'g<? w f 



~EKf 



(C.57) 



(C.58) 



(C.59) 



C.4.11 Maximum stress 

The maximum stresses under the different loading modes of all six case 
are listed below 

a = 3FL , 

amax 4^hJ 

_ 3w p L?L p F 

max ~ 2(w p h p Lf+2wfh,L i p ) 



3FHiLi 



wih? 



L l+ *±122± 



Ehf 



_ SFtR+H-H^Lt 



tU|/l? 



L,+ 9Ltca ?ri 



Ehf 



(C.60) 



ZR | 3w,LlL, 1 p 

Ih^fT 2(wfh t L p +2w p h p L?)\ r 



_3L 
AtvihJ 



+ 



3RL, 



wihf 



Ll+ lh22d 



~EhJ~ 
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Figure C.4: Cantilever beam subject to a force F and a moment M at its end 
point. 



Figure C.5: Force applied on the top of the hemisphere along the axis of the 



sensor 
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Figure C.6: Leg of the loadcell under a symmetrically applied vertical load 



^^* , ^ , w ff .», W! , ^^^ mm niim i i i i ojiilupi^^ 



r\ 



APPENDIX C. FINGERTIP SENSOR 



151 



S~\ 



KJ 



FigureC.7: AhoriioataliraekajBliedtethoont^oftlweeinoria line with 
the inner loadcdl 



«>•©, 






*n 



1 1 ' 

X^ 3^ 



'/« 



Figure C A The 
since the fence is 
and not rotate. 



nnder 



aw eataned to be rigid and 
to mow rigidly 



APPENDIX C. FINGERTIP SENSOR 



152 



Figure C.9: Horizontal force applied to the lowest portion of the fingertip shell 




Figure C.10: The outer phalanges are all assumed to be rigid. The major stress 
components are located on the upper and lower surfaces on the legs parallel to the 
externally applied force at the cantilevered end. 
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Figure C.ll: The legs parallel to the applied force are subject to both a force 
and a moment at their end points, while the legs perpendicular to the force are 
only subject to a pure torsional moment. 
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Figure C.12: The legs parallel to the applied force undergo a rotation at the end 
point of <f> and a displacement of <j>L h under an applied force f\ and a moment 
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Appendix D 

Slip analysis software 



D.l Introduction 

The software was not included just to add bulk to this master's thesis. 
In fact, the construction of the LISP functions along with the documen- 
tation are similar to the development of the chapters in the thesis. The 
high level functions are listed at the beginning of every section, while 
the lower level supporting function are listed in a separate section, one 
for each high level function. 
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; ; -*- Mode:LISP; Base:10; Syntax: Common-lisp; Package: USER -*- 

FILE: CONSTRAINT. LISP 

This file contains functions which analysis the relationship between a grasped ob j ! 
ect 

and grasping articulators. The file is divided into six major sections: 

1. General functions and variables 

This section contains functions and variables used by all the other 
sections in the file. Many of the variables will come from the sensing 
on the hand. For example *contact-points*, from the fingertip and 
joint sensors. Other variables must be specified, but may in the 
in the future be sensed as well. For instance, *cg* the center of 
gravity of the object, and *contact-surfaces* the local surface shape 
in the area of the contact . 

1 . 1 Define general global variables 

1.2 Set initial values of the general global variables 

1.3 General utility functions 

1.3.1 General math functions 

1.3.2 General matrix manipulation functions 

1.3.2.1 Major manipulation functions 

a. ADD-ARRAY-LIST 

b. MULTIPLY-ARRAY-LIST 
C . ROW-REDUCED-ECHELON-MATRIX 

d. TRANSFORM-DIRECTION-GLOBAL-FRAME 

e. TRANSFORM-DIRECTION-FRAME-GLOBAL 

f. TRANSFORM-POINTS-GLOBAL-FRAME 

g. TRANSFORM-POINTS-FRAME-GLOBAL 
h . TRANSFORM-FRAMES-GLOBAL-FRAME 
i. TRANSFORM-FRAMES-FRAME-GLOBAL 
j. GENERATE-FRAME 
k. GENERATE-FRAME-POINTS 

1.3.2.2 Auxiliary functions 

a. ADD-ARRAY-LIST 

b. MULTIPLY-ARRAY-LIST 
C . ROW-REDUCED-ECHELON-MATRIX 

d. TRANSFORM-DIRECTION-GLOBAL-FRAME 

e . TRANSFORM-D IRECT ION-FRAME -GLOBAL 

f. TRANSFORM-POINTS-GLOBAL-FRAME 

g. TRANSFORM-POINTS-FRAME-GLOBAL 
h. TRANSFORM-FRAMES-GLOBAL-FRAME 
i . TRANSFORM-FRAMES-FRAME-GLOBAL 
j . GENERATE-FRAME 
k. GENERATE-FRAME-POINTS 
1. General 

1.3.3 General matrix output functions 

1.3.3.1 Major matrix output functions 
a. PRINT-ARRAY-LIST 

1.3.3.2 Auxiliary functions 

a. PRINT-ARRAY-LIST 

b. General 

1.4 Graphics functions 

1.4.1 Define graphics variables 

1.4.2 Defind functions to set graphics variables 

1.4.3 Set graphics functions 

1.4.4 Graphics functions 
1.4.4.1 Screen creation and initialization functions 

a. MAKE-GRASP-SCREEN 



e: 



bles 



of 

t 

t 



b. KILL-GRASP-SCREEN 
C . START-MONITOR-MSG 

d. CREATE-GRASP-SCREEN 

e. CLEARSCREEN 
1.4.4.2 Drawing functions 

a. DRAW-2D-GRASP-WIND0W 

b. DRAW-3D-GRASP-WIND0W 

C . SPHERE-3D-GRASP-WINDOW 

d. DRAW-COORDINATE-SYSTEM 

e. DRAW-COORDINATES 

f. DRAW-3D-LIST 
1.4.5 Auxiliary graphics functions 

Sensed and global variables 

This section defines and initializes sensed and global variables used in 
the other sections. The only variables which are truely sensed externally ar! 

*contact-points* a list of contact-points in the hand space 
*contact-normals* a list of normals at the contact points in han! 

space . 

The force and tactile sensing fingertip need to be working for these variable! 

to be read. Until then these variables will have to be constructed. So there 
are a number of functions and variables which are used to construct the varia! 

*contact-points* and *contact -normals*. These functions and variables are 
temporary and are used only for simulation. When the sensors are connected 
these variables and functions will have to be removed. 

Temporary variables used to construct *contact-points* 

*contact-points-object-space* a list of contact points in the 

object space 
*contact-normals-object-space* a list of contact normals in the 

object space 
* c< 3* _ the center of gravity of the object 

*major-axis* the normal indicating the direction! 

major axis of an axisymmetric objec! 



3. Constraint functions 

This section contain functions which analysis the constraint created by 
the contacts on the grasped object. An infinitesimal analysis involving 
virtual work and a finite motion analysis involving the shape of contact 
surface are both included in determine the overall constraint imposed by 
the contacts 

3.1 Define constraint variables 

3.2 Set constraint variables 

3.2.1 Functions for setting constraint variables 

3.3 Constraint analysis functions 
3.3.1 Major analytic functions 

a . CONSTRUNT-VIRTUAL-WORK-LIST 

b. DETERMINE-CONTACT-TYPES 
3.3.1 Auxiliary functions 

4 . Body wrench 



g 

r 

set 



al 

r 

ts 



In this section the wrench on the object resulting from forces excluding 
contact forces from the robot is calculated. Body wrenches may result from 
gravity, accelerations, electromagnetic forces, and contacts other than 
fingertips. The only body wrenches calculated in the present program are 
those resulting from gravity. 

4 . 1 Define body wrench variables 

4.2 Set body wrench variables 

4.3 Body wrench functions 

4.3.1 Major body wrench functions 
a. ORIENTATION 

4.3.2 Auxiliary functions 
a. ORIENTATION 

5 . Contact wrenches 

The wrenches in the contact space for each contact are calculated assumin! 
a certain stiffness at the fingertips, a body wrench (section 3), and an off! 
wrench (section 6) . 

5 . 1 Define contact wrench variables 

5.2 Set contact wrench variables 

5.2.1 Functions to set contact wrench variables 

5.3 Contact wrench functions 

5.3.1 Major contact wrench functions 
a . CONS TUCT -CONTACT -WRENCH 

5.3.2 Auxiliary functions 

a . CONSTUCT-CONTACT-WRENCH 

6 . Contact types 

In this section the type of contact is determined given certain friction! 
criteria and the wrench in the contact space. The different types of contac! 

are 

(1) Soft finger contact 

(2) Point contact with friction 

(3) Point contact without friction 

(4) No contact 

6 . 1 Define contact type variables 

6.2 Set contact type variables 

6 . 3 Contact type functions 

6.3.1 Major contact type functions 
a. CONTACT-TYPE 

6.3.2 Auxiliary functions 
a. CONTACT-TYPE 

7 . Offset wrench 

The internal grasping force exerted by the contacts on an object are not 
determined by external forces and may be varied arbitarily on an object . 
The space of possible solutions to the grasping force problem varies with 
the number of contacts : 

Number of contacts Dimension of solution space 



ns 
* 

o 
} 

nd 



1 

2 1 

3 3 

4 6 

n 3*(n-2) n>=3 

For a two fingered grasp, the dimension of the solution space on the 
internal grasping forces is one. That is, in general, the squeeze force 
between the two fingers may be varied. 

For three fingers there is a three dimensional space of solutions. 
Conceptually this space can be simplified. If there are three forces 
exerted on an object, and at least one force is not parallel to the others, 
and the object is not accelerating, then the three 
forces intersect at a point in space. Furthermore, these intersection point! 

lie on the plane formed by the three contacts points. The three dimensional 
solution space of grasping forces is then a point (X,Y) lying on the graspin! 

force plane, and F the grasping force magnitude. 

For more than three fingers, the solution space grows by 3*(n-2), where n 
is the number of contacts . 

7 . 1 CONSTRUCT-OFFSET-WRENCH 

7 . 2 OFFSET-WRENCH-THREE-CONTACTS 
8 . Controlled slip 

This is an experimental section formed of functions which use the functio! 

of the previously descibed sections. The number of controllable variables t! 

execute dexterous control of an object within a grasp of a three fingered ha! 

is enormous: 

Controllable variables Dimension of space 

Grasping force 3 

Orientation 3 

Stiffness 9 

(more here) 

8.1 Define controlled slip variables 

8.2 Set controlled slip variables 

8.3 Controlled slip variables 

8.3.1 PERMISSIBLE TWIST 

accepts: twist 
assumes : orientation 

grasping force center 
returns : maximally constrained state which allows the 
allows the specified twist 

orientation 

grasping force magnitude 

8.3.2 DETERMINE-CONSTRAINT-STATE accepts: grasping force center 

grasp force magnitude 
assumes: orientation 
returns : constraint state 

8.3.3 MAP-CONSTRAINT-SPACE 



accepts : force-magnitude 
assumes: orientation 

stiffness 
returns: two-dimensional map of constrain states 

as a function of (x,y) the grasping force 

center 



9. Hand actuation functions 
This section contains functions to perform actual motions of the hand. 

9.1 Define actuations variables 

9.2 Define functions to set actuation functions 

9.3 Set actuation variables 

9.4 Grasp functions 

9.4.1 MOVE-TO-CONTACT-POINTS assumes: *contact-f rames* 

returns: moves fingers to points defined ! 

in *contact-frames* 

9.4.2 GRASP accepts: grasping force center 

grasp force magnitude 

assumes : stiffness 

returns: motion of the fingers 

9.5 Grasp auxiliary functions 

9.5.1 MOVE-TO-CONTACT-POINTS auxiliary functions 

9.5.2 GRASP auxiliary funcitons 

10 . Menu 
Standard motion options: 
Screen graphics options : 

CLEARSCREEN : clears "grasp-window" pane of "grasp-scr! 

screen 
DRAW COORDINATE SYSTEM :draws the three dimensional coordinate s! 

for the hand, object, and contacts 
MAP CONSTRAINT SPACE : plots the constraint states on the two 

dimensional grasp surface for a specif ie! 



een" 



ystems 



grasp force magnitude 



Controlled slip options: 



them 
e 



ates 



RESET GLOBAL VARIABLES : shows the current values of the global 

variables and allows the user to change ! 

DETERMINE CONSTRAINT STATE : accepts the grasping force center and th! 

grasping force magnitude 
assumes the stiffness and orientation 
and returns the constraint state 
PERMISSIBLE TWIST : accepts a twist 

assumes an orientation 

and returns the maximally constrained st ! 

which allows that twist 
Hand actuations options: 



ng to 



ose 

joy 
s the 



MOVE TO CONTACT POINTS 



PICK GRASP FORCE CENTER 



:moves fingers to the points cooresspondi ! 

those in *contact-frames* 
:plots the contacts on the two dimensional 

grasp surface and allows the user to cho! 

a particular force magnitude and use the! 

stick to pick a force center and actuate! 

hand accordingly 



Advanced hand actuation options 
CONTROLLED SLIP 



: allows the user to enter an object motio! 
and the hand tries to actuate it . 



he 



11. Demos 

This section contains demo programs which are based on the analyses of t ! 
the previous section. 



;;;*************************************************************************************j 

***** 



t r t 
*** * * 



1. General functions and variables 

*************************************************************************************! 



(defun sqr (x) 

(cond ((not (numberp x) ) 0) 
(t (* x x)))) 



1 . 1 Define variables 

1.2 Set default variable values section 

1.2.1 Functions to set default variable values 

1.2.2 Set default variable values 

1.3 General utility functions 

1.3.1 General math functions 
1.3.1.1 SQR 



1.3.2 General matrix manipulation functions 
1.3.2.1 ADD-ARRAY-LIST 



(defun add-array-list (array-list) 
(cond ((null array-list) nil) 

( (atom array-list) array-list) 

(t (add-two-arrays (add-array-list (car array-list) ) 

(add-array-list (cdr array-list)))))) 

1.3. 2.1. A ADD-ARRAY-LIST Auxiliary 
(defun add-two-arrays (array-1 array-2) 

(cond ((and (not array-1) (not array-2)) nil) 
((not array-1) array-2) 
((not array-2) array-1) 
( (vectorp array-1) 
(let* ((elements (array-dimension array-1 0)) 
(sum-array (make-array elements))) 
(do ((i (+ i 1))) 



)))) 



( (= i elements) sum-array) 
(setf (aref sum-array i) (+ (aref array-1 i) (aref array-2 i) ) ) ) ) ) 
(t (let* ((rows (array-dimension array-1 0)) 

(columns (array-dimension array-2 1)) 
(sum-array (make-array (list rows columns)))) 
(do (( i (+ i 1))) 

( (= i rows) sum-array) 
(do ((j (+ j 1))) 

( (= j columns) nil) 
(setf (aref sum-array i j) (+ (aref array-1 i j) (aref array-2 i j))))) 

1.3.2.2 MULTIPLY-ARRAY-LIST 



(defun multiply-array-list (array-list) 
(cond ( (null array-list) nil) 

( (atom array-list) array-list) 

(t (multiply-two-arrays (multiply-array-list (car array-list)) 

(multiply-array-list (cdr array-list) ) ) ) ) ) 

1.3. 2. 2. A MULTIPLY-ARRAY-LIST Auxiliary 
(defun multiply-two-vectors (array-1 array-2) 
(let* ( (elements-1 (array-dimension array-1 0)) 
(elements-2 (array-dimension array-2 0) ) 

(product-array (initialize-array elements-1 elements-2))) 
(cond ((not (= elements-1 elements-2)) nil) 
(t 
(do ((j (+ j 1))) 

( (•■ j elements-1) product-array) 
(do <(k (+ k 1))) 

( (= k elements-1) nil) 
(setf (aref product-array j k) 

(* (aref array-1 j) (aref array-2 k) ))))))) ) 

(defun multiply-array-vector (array vector) 
(let* ((rows-1 (array-dimension array 0)) 

(elements (array-dimension vector 0) ) 
(product-array (initialize-vector elements))) 
(do ((j (+ j 1))) 

( (= j rows-1) product-array) 
(do ((k (+ k 1))) 

( (= k elements) nil) 
(setf (aref product-array j) 

(+ (aref product-array j) 

(* (aref array j k) (aref vector k) )))))) ) 

(defun multiply-two-arrays (array-1 array-2) 
(cond ((and (not array-1) (not array-2)) nil) 
((not array-1) array-2) 
( (not array-2) array-1) 

((and (not (arrayp array-1)) (not (arrayp array-2))) (* array-1 array-2)) 
((not (arrayp array-1)) (multiply-constant-array array-1 array-2)) 
((not (arrayp array-2)) (multiply-constant-array array-2 array-1)) 
( (vectorp array-1) 
(cond ((not (= (array-rank array-2) 1)) nil) 

(t (multiply-two-vectors array-1 array-2)))) 
((not (equalp (array-dimension array-1 1) 

(array-dimension array-2 0))) nil) 
( (vectorp array-2) (multiply-array-vector array-1 array-2) ) 
(t (let* ((rows-1 (array-dimension array-1 0)) 
(rows-2 (array-dimension array-2 0) ) 
(columns-2 (array-dimension array-2 1) ) 
(product-array (initialize-array rows-1 columns-2))) 
(do (( i (+ i 1))) 

( (= i columns-2) product-array) 
(do ((j (+ j 1))) 

( (= j rows-1) nil)p 



(do ((k (+ k 1))) 

( (= k rows-2) nil) 
(setf (aref product-array j i) 

(+ (aref product-array j i) 

(* (aref array-1 j k) (aref array-2 k i) ))))))))) ) 

(defun multiply-constant-array (constant array) 
(cond ( (vectorp array) 

(let* ( (elements (array-dimension array 0) ) 

(product-array (make-array elements) ) ) 
(do ((i (+ i 1))) 

( (= i elements) product-array) 
(setf (aref product-array i) (* constant (aref array i) ) ) ) ) ) 
(t 
(let* ( (rows (array-dimension array 0) ) 

(columns (array-dimension array 1) ) 
(product-array (make-array (list rows columns) ) ) ) 
(do <(i (+ i 1))) 

( (= i rows) product-array) 
(do ((j (+ j 1))) 

( (= j columns) nil) 
(setf (aref product-array i j) (* constant (aref array i j)) ))))))) 

1.3.2.3 ROW-REDUCED-ECHELON 
(defun row-reduced-echelon (array) 

(let ((rows (array-dimension array 0)) 

(columns (array-dimension array 1))) 
(row-reduced-echelon-aux (diagonalize array) ( - rows 1) rows columns))) 

1.3. 2. 3. A ROW- REDUCED -ECHELON Auxiliary 
(defun zero-array (rows columns) 

(cond ((or (< rows 0) (< columns 0)) nil) 
((and (= rows 0) (= columns 0)) nil) 
( (= rows 0) (zero-vector columns)) 
( (= columns 0) (zero-vector rows)) 
(t 
(let ((array (make-array (list rows columns)))) 
(do ((i (+ i 1))) 

( (= i rows) array ) 
(do ((j (+ j 1))) 
( (= j columns) ) 
(setf (aref array i j) 0))))))) 

(defun zero-vector (elements) 
(cond ( (<= elements 0) nil) 
(t 
(let ((vector (make-array elements))) 
(do (<i (+ i 1))) 

( (= i elements) vector) 
(setf (aref vector i) 0)))))) 

(defun row-reduced-echelon-aux (array current-row current -column rows columns) 
(cond ( (= current-row 0) array) 

((zero-row array current -column current-row) 

(row-reduced-echelon-aux array (- current-row 1) rows columns) ) 
( (= current-column columns) 

(row-reduced-echelon-aux array (- current-row 1) rows columns) ) 
( (< (sqr (aref array current-row current-column)) 0.00001) 

(row-reduced-echelon-aux array current-row (+ 1 current-column) rows columns) ) 
(t (row-reduced-echelon-aux 

(row-reduced-echelon-aux-1 array current-row current-row current-column row! 
s ) 

(- current-row 1) 
rows columns) ) ) ) 

(defun row-reduced-echelon-aux-1 (array inc-row current-row current-column rows) 



(cond ( (= inc-row 0) array) 
(t 
(row-reduced-echelon-aux-1 
(add-x-times-rl-row-to-r2-row array current-row current-column (- inc-row 1) ) 
(- inc-row 1) current-row current-column rows) ) ) ) 

(defun diagonalize (array) 

(let ( (rows (array-dimension array 0) ) 

(columns (array-dimension array 1) ) ) 
(diagonalize-array-aux array rows columns) ) ) 

(defun zero-column (array current-column current-row) 
(let ((rows (array-dimension array 0)) 
(sum 0) ) 
(do ( (i current-row (+ i 1) ) ) 

( (= i rows) (< (sqr sum) 0.00001)) 
(setq sum (+ sum (abs (aref array i current-column))))))) 

(defun zero-row (array current-column current-row) 
(let ( (columns (array-dimension array 1) ) 
(sum 0) ) 
(do ((i current-column (+ i 1))) 

( (= i columns) (< (sqr sum) 0.00001)) 
(setq sum (+ sum (abs (aref array current-row i) ) ) ) ) ) ) 

(defun diagonalize-array-aux (array current-column current-row rows columns) 
(cond ( (= current-column (min rows columns) ) array) 

( (zero-column array current-column current-row) 

(diagonalize-array-aux array (+ 1 current-column) current-row rows columns) ) 
(t 
(diagonalize-array-aux 

(initialize-column-aux 

(divide-r-row-by-element-rc array current-row current-column) 
current-column current-row (+ 1 current-row) rows) 
(+ 1 current -column) 
(+ 1 current-row) 
rows 
columns) ) ) ) 

(defun initialize-column-aux (array initial-column initial-row current-row rows) 
(cond ( (= current-row rows) array) 
(t 
(initialize-column-aux 

(add-x-times-rl-row-to-r2-row array 

initial-row initial-column current-row) 
initial-column 
initial-row 
(+ 1 current-row) 
rows) ) ) ) 

(defun divide-r-row-by-element-rc (array r c) 
(let ( (columns (array-dimension array 1) ) 
(first-element (aref array r c) ) 
(new-array (initialize array) ) ) 
(cond ( (< (sqr first-element) 0.00001) 

(divide-r-row-by-element-rc (interchange-last-row-and-row-r array r) re)) 
(t 
(do ((i c (+ i 1))) 

( (= i columns) new-array) 
(setf (aref new-array r i) (/ (aref array r i) first-element))))))) 

(defun add-x-times-rl-row-to-r2-row (array rl cl r2) 
(cond ( (< (sqr (aref array rl cl) ) 0.00001) array) 
(t 
(let ( (columns (array-dimension array 1) ) 



(new-array (initialize array) ) 
(constant (* -1 (aref array r2 cl)))) 
(do ((icl (+ i 1))) 

( (= i columns) new-array) 
(setf (aref new-array r2 i) (+ (* constant (aref array rl i) ) 

(aref array r2 i) )))))) ) 

(defun interchange-last-row-and-row-r (array r) 
(let ((last-row (- (array-dimension array 0) 1)) 
(columns (array-dimension array 1)) 
(new-array (initialize array) ) ) 
(do ((i (+ i 1))) 

( (= i columns) new-array) 
(setf (aref new-array r i) (aref array last-row i) ) 
(setf (aref new-array last-row i) (aref array r i) ) ) ) ) 

(defun transpose-contact-frame (contact-frame) 

(let* ( (t-contact-f rame (initialize contact-frame))) 
(do ((i (+ i 1))) 

( (= i 3) t-contact-f rame) 
(do <(j (+ j 1))) 
((= j 3)) 
(setf (aref t-contact-frame i j) (aref contact-frame j i) ) ) ) ) ) 

1.3.2.4 TRANS FORM-DIRECT I ON-GLOBAL -FRAME 
(defun transform-direction-global-frame (frames objects) 
(cond ( (and (listp objects) (listp frames) ) 

(transform-direction-global-frame-lists frames objects)) 
( (listp objects) 

(transform-direction-global-frame-list frames objects) ) 
(t 
(transform-direction-object-global-frame frames objects)))) 

1.3. 2. 4. A TRANSFORM-DIRECTION-GLOBAL-FRAME Auxi ! 
liary 

(defun transform-direction-global-frame-lists (frames objects) 
(cond ( (null frames) nil) 

(t (cons (transform-direction-object-global-frame (car frames) (car objects) ) 

(transform-direction-global-frame-lists (cdr frames) (cdr objects) ) ) ) ) ) 

(defun transform-direction-global-frame-list (frame objects) 
(cond ( (null objects) nil) 

(t (cons (transform-direction-object-global-frame frame (car objects) ) 

(transform-direction-global-frame-list frame (cdr objects) ) ) ) ) ) 
(defun transform-direction-object-global-frame (frame object) 

(cond ( (vectorp object) (transform-direction-vector-global-frame frame object) ) 
( (arrayp object) (transform-direction-array-global-frame frame object) ) 
(t nil))) 

(defun transform-direction-array-global-frame (frame array) 
(let* ( (rows (array-dimension array 0) ) 

(columns (array-dimension array 1) ) 
(new-array (make-array (list rows columns) ) ) ) 
(do ((i (+ i 3))) 

( (= i rows) new-array) 
(do ((j (+ j 1))) 
( (= j columns) ) 
(do (<k (+ k 1))) 
((= k 3)) 
(setf (aref new-array (+ i k) j) 

(+ (* (aref array i j) (aref frame k) ) 
(* (aref array (+ i 1) j) (aref frame 1 k) ) 
(* (aref array (+ i 2) j) (aref frame 2 k) ))))))) ) 

(defun transform-direction-vector-global-frame (frame vector) 
(let* ((elements (array-dimension vector 0)) 



(new-vector (make-array elements) ) ) 
(do (<i (+13))) 

( (= 1 elements) new-vector) 
(do <(k (+ k 1))) 
((= k 3)) 
(setf (aref new-vector (+ 1 k) ) 

(aref vector 1) (aref 
(aref vector (+ i 1) ) (aref 
(aref vector (+12)) (aref 



(aref 

(+ (* 

(* 

(* 



frame 
frame 
frame 



k)) 
k)) 
k)))))))) 



1.3.2.5 TRANSFORM-DIRECTION-FRAME-GLOBAL 
(defun transform-direction-frame-global (frames objects) 
(cond ((and (listp objects) (listp frames)) 

(transform-direction-frame-global-lists frames objects) ) 
( (listp objects) 

(transform-direction-frame-global-list frames objects) ) 
(t 
(transform-direction-object-frame-global frames objects)))) 

1.3. 2. 5. A TRANSFORM-DIRECTION-FRAME-GLOBAL Auxi ! 
liary 

(defun transform-direction-frame-global-lists (frames objects) 
(cond ( (null frames) nil) 

(t (cons (transform-direction-object-frame-global (car frames) (car objects)) 

(transform-direction-frame-global-lists (cdr frames) (cdr objects)))))) 

(defun transform-direction-frame-global-list (frame objects) 
(cond ( (null objects) nil) 

(t (cons (transform-direction-object-frame-global frame (car objects) ) 

(transform-direction-frame-global-list frame (cdr objects) ) ) ) ) ) 

(defun transform-direction-object-frame-global (frame object) 

(cond ( (vectorp object) (transform-direction-vector-frame-global frame object) ) 
( (arrayp object) (transform-direction-array-frame-global frame object) ) 
(t nil))) 



(defun transform-direction-array-frame-global (frame array) 
(let* ((rows (array-dimension array 0)) 

(columns (array-dimension array 1) ) 
(new-array (make-array (list rows columns) ) ) ) 
(do ((i (+13))) 

( (= i rows) new-array) 
(do ((j (+ j 1))) 
( (= j columns) ) 
(do (<k (+ k 1))) 
((= k 3)) 
(setf (aref new-array (+ i k) j) 

(aref array i j) (aref frame k 
(aref array (+ i 1) j) (aref frame k 
(aref array (+12) j) (aref frame k 



(aref 

(+ <* 

(* 

(* 



0)) 
D) 
2))))))))) 



(defun transform-direct ion- vector-frame-global 
(let* ((elements (array-dimension vector 0)) 
(new-vector (make-array elements) ) ) 
(do <(i (+13))) 

( (= i elements) new-vector) 
(do (<k (+ k 1))) 
((= k 3)) 
(setf (aref new-vector (+ i k) ) 
(+ (* (aref vector i) 
(* (aref vector (+ i 1) ) 

(* (aref vector (+ i 2)) (aref frame 



(frame vector) 



(aref frame 
(aref frame k 



k 0)) 

D) 

k 2)))))))) 



1.3.2.6 TRANSFORM-POINTS-GLOBAL-FRAME 
(defun transform-points-global-frame (frames objects) 
(cond ( (and (listp objects) (listp frames) ) 



(transform-points-global-frame-lists frames objects) ) 
((listp objects) 

(transform-points-global-frame-list frames objects)) 
(t 

(transform-points-object-global-frame frames objects) ) ) ) 

1.3. 2. 6. A TRANSFORMP-POINTS-FRAME-GLOBAL Auxili ! 
ary 

(defun transform-points-global-frame-lists (frames objects) 
(cond ((null frames) nil) 

(t (cons (transform-points-object-global-frame (car frames) (car objects) ) 

(transform-points-global-frame-lists (cdr frames) (cdr objects) ) ) ) ) ) 

(defun transform-points-global-frame-list (frame objects) 
(cond ( (null objects) nil) 

(t (cons (transform-points-object-global-frame frame (car objects) ) 

(transform-points-global-frame-list frame (cdr objects)))))) 

(defun transform-points-object-global-frame (frame object) 

(cond ( (vectorp object) (transform-points-vector-global-frame frame object)) 
( (arrayp object) (transform-points-array-global-frame frame object)) 
(t nil))) 



(defun transform-points-array-global-frame (frame array) 
(let* ((rows (array-dimension array 0)) 

(columns (array-dimension array 1) ) 
(new-array (make-array (list rows columns) ) ) ) 
(do ((i (+ i 3))) 

( (= i rows) new-array) 
(do <(j (+ j 1))) 
( (= j columns) ) 
(do (<k (+ k 1))) 
((= k 3)) 
(setf (aref new-array (+ i 
(- (aref array 
(- (aref array 
(- (aref array 



(aref 

(+ (* 

(* 

(* 



k) j) 

i j) (aref 
(+ i 1) j) 
(+ i 2) j) 



frame 
(aref 
(aref 



3)) 
frame 
frame 



3)) 
3)) 



(aref 
(aref 
(aref 



frame 
frame 
frame 



k)) 
k)) 
k))))))) ! 



)) 



(defun transform-points-vector-global-frame (frame vector) 
(let* ((elements (array-dimension vector 0)) 
(new-vector (make-array elements) ) ) 
(do ||i0 (+ i 3))) 

( (= i elements) new-vector) 
(do <(k (+ k 1))) 
((= k 3)) 
(setf (aref new-vector (+ i k) ) 

(- (aref vector i) (aref frame 3) ) 
(- (aref vector (+ i 1) ) (aref frame 1 
(- (aref vector (+ i 2)) (aref frame 2 



(aref 

(+ (* 

(* 

(* 



3)) 
3)) 



(aref 
(aref 
(aref 



frame 
frame 
frame 



k)) 
k)) 
k)))))))) 



1.3.2.7 TRANSFORM-POINTS-FRAME-GLOBAL 
(defun transform-points-frame-global (frames objects) 
(cond ((and (listp objects) (listp frames)) 

(transform-points-frame-global-lists frames objects) ) 
( (listp objects) 

(transform-points-frame-global-list frames objects) ) 
(t 
(transform-points-object-frame-global frames objects) ) ) ) 



1.3. 2. 7. A TRANSFORM-POINTS-FRAME-GLOBAL Auxilia ! 
ry 

(defun transform-points-frame-global-lists (frames objects) 
(cond ( (null frames) nil) 

(t (cons (transform-points-object-frame-global (car frames) (car objects)) 

(transform-points-frame-global-lists (cdr frames) (cdr objects) ) ) ) ) ) 



(defun transform-points-frame-global-list (frame objects) 
(cond ( (null objects) nil) 

(t (cons (transform-points-object-frame-global frame (car objects)) 

(transform-points-frame-global-list frame (cdr objects) ) ) ) ) ) 

(defun transform-points-object-frame-global (frame object) 

(cond ( (vectorp object) (transform-points-vector-frame-global frame object) ) 
( (arrayp object) (transform-points-array-frame-global frame object) ) 
(t nil))) 

(defun transform-points-array-frame-global (frame array) 
(let* ( (rows (array-dimension array 0) ) 

(columns (array-dimension array 1) ) 
(new-array (make-array (list rows columns)))) 
(do <<i (+ i 3))) 

( (= i rows) new-array) 
(do ((j (+ j 1))) 
( (= j columns) ) 
(do <(k (+ k 1))) 
((= k 3)) 
(setf (aref new-array (+ i k) j) 

(aref array i j) (aref frame 
(aref array (+ i 1) j) (aref frame 
(aref array (+ i 2) j) (aref frame 



(aref 

(+ (* 

(* 

(* 



0)) 
1>> 
2)) 



(aref frame k 3) )))))) ) 



(defun transform-points-vector-frame-global (frame vector) 
(let* ((elements (array-dimension vector 0)) 
(new-vector (make-array elements) ) ) 
(do <<i (+ i 3))) 

( (= i elements) new-vector) 
(do ((k (+ k 1))) 
((= k 3)) 
(setf (aref new-vector (+ i k) ) 

(aref vector i) (aref frame k 0) ) 
(aref vector (+ i 1) ) (aref frame k 1) ) 
(aref vector (+ i 2) ) (aref frame k 2)) 



(aref 

(+ (* 

(* 

(* 



(aref frame k 3) ) ) ) ) ) ) 

1.3.2.8 TRANSFORM-FRAMES-GLOBAL-FRAME 
(defun transform-frames-global-frame (frames object-frames) 
(cond ((and (listp object-frames) (listp frames)) 

(transform-frames-global-frame-lists frames object-frames) ) 
((listp object-frames) 

(transform-frames-global-frame-list frames object-frames)) 
(t 
(transform-frame-global-frame frames object-frames) ) ) ) 

1.3. 2. 8. A TRANSFORM-FRAMES -GLOBAL-FRAME Auxilia ! 
ry 

(defun transform-frames-global-frame-lists (frames object-frames) 
(cond ( (null frames) nil) 

(t (cons (transform-frames-global-frame (car frames) (car object-frames) ) 

(transform-frames-global-frame-lists (cdr frames) (cdr object-frames) ) ) ! 
))) 



(defun transform-frames-global-frame-list (frame object-frames) 
(cond ( (null object-frames) nil) 

(t (cons (transform-frame-global-frame frame (car object-frames) ) 

(transform-frames-global-frame-list frame (cdr object-frames) ) ) ) ) ) 

(defun transform-frame-global-frame (frame object-frame) 
(let* ((new-object-frame (make-array '(3 4)))) 
(do <(i (+ i 1))) 
((= i 3)) 



(do 



<<j 
((= 
(setf 



(do 



<(i 
((= 
(setf 



i) 
i) 



1 
2 



(+ j 1))) 

j 3)) 

(aref new-object-frame j 

(+ (* (aref object-frame 
(* (aref object-frame 
(* (aref object-frame 
i 1))) 

new-object-frame) 
(aref new-object-frame i 3) 

(+ (* (- (aref object-frame 
(* (- (aref object-frame 
(* (- (aref object-frame 



( + 
3) 



i) 
i) 



(aref 
(aref 
(aref 



frame 
frame 
frame 



j)) 
j)) 
j)))))) 



3) 
3) 
3) 



(aref frame 
(aref frame 
(aref frame 



3)) 
3)) 
3)) 



(aref frame 
(aref frame 
(aref frame 



i)> 
D) 
i) )))))) 



1.3.2.9 TRANSFORM-FRAMES-FRAME-GLOBAL 
(defun transform-frames-frame-global (frames object-frames) 
(cond ( (and (listp object-frames) (listp frames) ) 

(transform-frames-frame-global-lists frames object-frames) ) 
( (listp object-frames) 

(transform-frames-frame-global-list frames object-frames) ) 
(t 
(transform-frame-frame-global frames object-frames) ) ) ) 

1.3. 2. 9. A TRANSFORM-FRAMES-FRAME-GLOBAL Auxilia ! 
ry 

(defun transform-frames-frame-global-lists (frames object-frames) 
(cond ( (null frames) nil) 

(t (cons (transform-frame-frame-global (car frames) (car object-frames) ) 

(transform-frames-frame-global-lists (cdr frames) (cdr object-frames)))! 
))) 



(defun transform-frames-frame-global-list (frame object-frames) 
(cond ( (null object-frames) nil) 

(t (cons (transform-frame-frame-global frame (car object-frames) ) 

(transform-frames-frame-global-list frame (cdr object-frames) ) ) ) ) ) 



(defun transform-frame-frame-global (frame 
(let* ( (new-object-frame (make-array ' (3 
(do <(i (+ i 1))) 
((= i 3)) 
(do ((j (+ j 1))) 
((= j 3)) 
(setf (aref new-object-frame j 
(aref object-frame 
(aref object-frame 
(aref object-frame 
(do ((i (+ i 1))) 

((= i 3) new-object -frame) 

(setf (aref new-object-frame i 3) 

(+ (* (aref object-frame 

(* (aref object-frame 

(* (aref object-frame 

(aref frame i 3) ) ) ) ) ) 



object- 
4)))) 



(aref 

(+ (* 

(* 

(* 



i) 

1 
2 




1 
2 



•frame) 



i) 
i) 
i) 



3) 
3) 
3) 



(aref 
(aref 
(aref 



(aref 
(aref 
(aref 



frame 
frame 
frame 



frame 
frame 
frame 



0)) 
D> 
2)))))) 



0)) 
D) 
2)) 



1.3.2.10 GENERATE-FRAME 
(defun generate-f rame (points vectors) 
(cond ( (null points) nil) 

( (atom points) (generate-frame-aux points vectors) ) 
(t (cons (generate-f rame (car points) (car vectors) ) 

(generate-frame (cdr points) (cdr vectors)))))) 

1.3.2.10 GENERATE-FRAME Auxiliary 
(defun generate-frame-aux (point vector) 
(let* ((frame (make-array '(3 4))) 

(length (sqrt (+ (sqr (aref vector 0)) 
(sqr (aref vector 1) ) 
(sqr (aref vector 2))))) 



(nx (/ (aref vector 0) length)) 
(ny (/ (aref vector 1) length)) 



(nz (/ (aref vector 

(lx) (ly) (lz) (mx) 

(cond ( (- nx 0) (setq lx 

( (= ny 0) (setq lx 



(setq 
(setq 
(setq 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
frame 



(t (setq lx 

(setq ly 

(setq lz 

(- (* ny 

(- (* nz 

mz (- (* nx 

(aref frame 

(aref frame 



mx 
my 



2) length)) 
(my) (mz)) 
1) (setq ly 
0) (setq ly 
(sqrt (/ (sqr (/ ny nx) ) 
(/ (* -1 nx lx) ny)) 
0))) 

(* nz ly))) 



0) (setq lz 0) ) 
-1) (setq lz 0) ) 

(+ 1 (sqr (/ ny nx) ) ) ) ) ) 



lz) 
lx) 

iy) 

0) 



(aref 
(aref 



frame 
frame 



(aref frame 
(aref frame 



(aref 
(aref 
(aref 
(aref 
(aref 



frame 
frame 
frame 
frame 
frame 



(aref frame 



1) 
2) 
3) 
0) 
1) 
2) 
3) 
0) 
1) 
2) 
3) 



(* nx lz))) 
(* ny lx))) 

lx) 

mx) 

nx) 

(aref point 0) ) 

iy) 

my) 
ny) 

(aref point 1) ) 

lz) 

mz) 

nz) 

(aref point 2) ) 



)) 



1.3.2.11 GENERATE-FRAME-POINTS 
(defun generate-frame-points (points-1 points-2) 
(cond ( (null points-1) nil) 

( (atom points-1) (generate-f rame-points-aux points-1 points-2) ) 
(t (cons (generate-frame-points (car points-1) (car points-2)) 

(generate-frame-points (cdr points-1) (cdr points-2)))))) 



1.3.2.11 GENERATE-FRAME-POINTS Auxiliary 
(defun generate-f rame-points-aux (point-1 point-2) 

(let* ((length (sqrt (+ (sqr (- (aref point-2 0) (aref point-1 0))) 

(sqr (- (aref point-2 1) (aref point-1 1))) 
(sqr (- (aref point-2 2) (aref point-1 2)))))) 



(nx (/ (- (aref point-2 0) 
(ny (/ (- (aref point-2 1) 
(nz (/ (- (aref point-2 2) 
(normal (make-array ' (3) ) ) ) 

(setf (aref normal 0) nx) 

(setf (aref normal 1) ny) 

(setf (aref normal 2) nz) 

(generate-f rame-aux point-1 normal) ) ) 



(aref point-1 0) ) 
(aref point-1 1) ) 
(aref point-1 2) ) 



length) ) 
length) ) 
length) ) 



(defun count-atoms (list) 
(cond ((null list) 0) 
( (atom list) 1) 
( (+ (count -atoms 
(count-atoms 



(car list) ) 
(cdr list)))))) 



1.3.3 General-functions 
1.3.3.1 COUNT -ATOMS 



(defun initialize-vector (elements) 
(let ((vector (make-array elements))) 
(do ((i (+ i 1))) 

( (= i elements) vector) 
(setf (aref vector i) 0)))) 



1.3.3.2 INITIALIZE-VECTOR 



1.3.3.3 INITIALIZE-ARRAY 
(defun initialize-array (rows columns) 

(let ((array (make-array (list rows columns)))) 
(do (<i (+ i 1))) 



( (= i rows) array) 

(do <(j (+ j 1))) 

( (= j columns) nil) 

(setf (aref array i j) 



0))))) 



1.3.3.4 INITIALIZE 
(defun initialize (array) 

(let* ( (rows (array-dimension array 0) ) 

(columns (array-dimension array 1) ) 
(new-array (make-array (list rows columns)))) 
(do ((i (+ i 1))) 

( (= i rows) new-array ) 
(do ((j (+ j 1))) 
( (= j columns) ) 
(setf (aref new-array i j) (aref array i j)))))) 



1.3.3.5 TRANSPOSE 
(defun transpose (array) 

(let* ((rows (array-dimension array 0)) 

(columns (array-dimension array 1) ) 



(new-array (make-array 
(do ((i (+ i 1))) 

( (= i rows) new-array) 
(do ((j (+ j 1))) 
( (= j columns) ) 
(setf (aref new-array j 



(list columns rows) ) ) ) 



i) (aref array i j)))))) 



1.3.3.6 GET-ARRAY-COLUMN 
(defun get -array-column (array column) 

(let* ( (rows (array-dimension array 0) ) 
(vector (make-array rows))) 
(do ||i0 (+ 1 i))) 

( (= i rows) vector) 
(setf (aref vector i) (aref array i column) ) ) ) ) 

1.3.3.7 ABS-ARRAY 

(defun abs-array (array-list) 
(cond ((null array-list) nil) 

( (atom array-list) (abs -one-array array-list) ) 
(t (cons (abs-array (car array-list)) 

(abs-array (cdr array-list) ) ) ) ) ) 

(defun abs-one-array (array) 

(cond ( (vectorp array) (abs-one-vector array) ) 

(t (let* ((rows (array-dimension array 0)) 

(columns (array-dimension array 1) ) 
(new-array (make-array (list rows columns)))) 
(do ((i (+ i 1))) 

( (= i rows) new-array) 
(do ((j (+ j 1))) 
( (= j columns) ) 
(setf (aref new-array i j) (abs (aref array i j)) ))))))) 

(defun abs-one-vector (vector) 

(let* ((elements (array-dimension vector 0)) 
(new-vector (make-array elements))) 
(do ((i (+ i 1))) 

( (= i elements) new-vector) 
(setf (aref new-vector i) (abs (aref vector i) ) ) ) ) ) 



1.3. 3. 7. A ABS-ARRAY Auxiliary 
1.3.4 General matrix output functions 
1.3.4.1 PRINT-ARRAY 
(defun print-array (array-list) 

(cond ( (listp array-list) (print-array-list array-list)) 



(t (print-one-array array-list) ) ) ) 

1.3. 4.1. A PRINT-ARRAY Auxiliary 
(defun print-array-list (array-list) 
(cond ((null array-list) nil) 

(t (print-array (car array-list) ) 

(print-array-list (cdr array-list))))) 

(defun print-one-array (array) 

(cond ( (vectorp array) (print-vector array) ) 
(t (print-2d-array array) ) ) ) 

(defun print-2d-array (2d-array) 
(write-char #\newline) 
(write-char #\newline) 
(let ( (rows (array-dimension 2d-array 0) ) 

(columns (array-dimension 2d-array 1) ) ) 
(do <(i (+ i 1))) 
( (= i rows) nil) 
(do <<j (+ j 1))) 

( (= j columns) nil) 
(prinl (aref 2d-array i j)) 
(write-char #\space ) ) 
(write-char #\newline) ) ) ) 

(defun print-vector (vector) 
(write-char #\newline) 
(write-char #\newline) 

(let ((elements (array-dimension vector 0))) 
(do ((i (+ i 1))) 

( (= i elements) nil) 
(prinl (aref vector i) ) 
(write-char #\space ) ) ) ) 

; 1.4 Graphics functions 

1.4.1 Define graphics variables 



(defvar 
(defvar 
(defvar 
(defvar 
(defvar 
(defvar 



grasp-screen) 

grasp-window) 

view-frame) 

scale-2d) 

x-origin-2d) 

y-origin-2d) 



(defvar scale-3d) 
(defvar x-origin-3d) 
(defvar y-origin-3d) 
(defvar angle-x) 
(defvar angle-z) 



es 

(defun construct-view-frame 
(let* ( (ct (cos angle-x) ) 
(st (sin angle-x) ) 
(cp (cos angle-z) ) 
(sp (sin angle-z)) 
(view-frame (zero-array 3 



1.4.2 
1.4.2. 

(angle-x angle-z) 



Set default variable values section 

1 Define functions to set graphics variabl! 



4))) 



(setf 


(aref 


view-frame 





0) 


cp) 




(setf 


(aref 


view-frame 





1) 


(* -1 


ct sp) ) 


(setf 


(aref 


view- frame 





2) 


(* -1 


st sp) ) 


(setf 


(aref 


view-frame 


1 


0) 


sp) 




(setf 


(aref 


view-frame 


1 


1) 


(* ct 


cp)) 


(setf 


(aref 


view-frame 


1 


2) 


(* st 


cp)) 


(setf 


(aref 


view-frame 


2 


0) 


0) 





(setf (aref view-frame 2 1) (* -1 st) ) 
(setf (aref view-frame 2 2) ct) 
view-frame) ) 



(setq scale-2d 100) 
(setq x-origin-2d 300) 
(setq y-origin-2d 300) 

(setq scale-3d 200) 
(setq x-origin-3d 300) 
(setq y-origin-3d 500) 
(setq angle-x 0.400) 
(setq angle-z 0.400) 



1.4.2.2 Set graphics variables 



(setq view-frame (construct -view-frame angle-x angle-z) ) 

; 1.4.3 Graphics functions 

; 1.4.3.1 Screen definition and initialization fun! 

ctions 

1.4.3.1.1 MAKE-GRASP -SCREEN 
(defun make-grasp-screen (Soptional (proc-msg t) ) 
(if (create-grasp-screen) 

(progn (send grasp-screen : activate) 
(send grasp-screen : expose) 

(send grasp-screen :send-pane 'top-pane rselect)) 
(progn (send grasp-screen : select) 

(send grasp-screen :send-pane 'top-pane :select))) 
(if proc-msg ( start -monitor-msg) ) ) 

1.4.3.1.2 KILL-GRASP-SCREEN 
(defun kill-grasp-screen () 

(if (variable-boundp grasp-screen) 

(progn (if (variable-boundp pc) (send pc :kill) ) 

(send (send grasp-screen :send-pane 'bottom-pane : process) : reset) 
(send grasp-screen :kill) 
(variable-makunbound grasp-screen) 
T) 
nil)) 

; this function resets the bottom pane lisp listener and starts a the message monitor 
; use (monitor-msg pc tg 'verbose) if you want verbose message processing... 

1.4.3.1.3 FORCE-KBD-INPUT-STRING 
(defun force-kbd-input-string (window string) 

(loop for i from below (string-length string) 

do (send window : force-kbd- input (char-int (char string i) ) ) ) ) 

1.4.3.1.4 START-MONITOR-MSG 
(defun start -monitor-msg () 

(if (variable-boundp pc) 

(progn (send (send grasp-screen :send-pane 'bottom-pane rprocess) : reset) 
(send grasp-screen :send-pane 'bottom-pane ' : clear-screen) 
(force-kbd-input-string (send grasp-screen :get-pane 'bottom-pane) 

"(monitor-msg pc tg) " ) 
) 
(send grasp-screen :send-pane 'bottom-pane : line-out 
#. (ZL: STRING "Parallel connection doesn't exist. Can't start message processor"))) 
T) 



1.4.3.1.5 CREATE-GRASP-SCREENt 
(defun create-grasp-screen () 

(setq grasp-screen (tvrmake-window 

'tv:bordered-constraint-f rame 



)))) 



' : panes 

'( (top -pane tv: window-pane 

: label "MAIN LISP LISTENER" 
:more-p nil) 
(bottom-pane tv: window-pane 

: label "MESSAGE PROCESSOR" 
:more-p nil) 
(grasp-window tv: window 

: label "GRASP WINDOW" 
:activate-p t) ) 
' : configurations 
' ( (main-conf ig 
( : layout 

(main-conf ig :row grasp-window right-side) 
(right-side :column top-pane bottom-pane)) 
( :sizes 

(right-side (top-pane :even) (bottom-pane :even)) 
(main-conf ig (right-side 0.3) :then (grasp-window :even) ) ) ) ! 



1.4.3.2 CLEARSCREEN 
(defun clearscreen () 

(send grasp-screen :send-pane 'grasp-window ': refresh)) 

'' 1.4.3.3 Draw functions 

1.4.3.3.1 DRAW-2D-GRASP-WINDOW 
(defun draw-2d-grasp-window (starting-point ending-point) 

(let ( (x-start) (y-start) 
(x-end) (y-end) 
(window-height) ) 
(setq window-height (send grasp-screen :send-pane 'grasp-window ':height)) 
(setq x-start (round (+ x-origin-2d (* scale-2d (aref starting-point 0))))) 
(setq y-start (round (+ window-height 

(* -1 (+ y-origin-2d (* scale-2d (aref starting-point 1) ) ) ) ) ! 

(setq x-end (round (+ x-origin-2d (* scale-2d (aref ending-point 0))))) 
(setq y-end (round (+ window-height (* -1 (+ y-origin-2d (* scale-2d (aref ending-po! 
int 1))))))) 

(send grasp-screen :send-pane 'grasp-window ' :draw-line x-start y-start x-end y-end) ! 
)) 

1.4.3.3.2 DRAW-3D-GRASP-WINDOW 
(defun draw-3d-grasp-window (starting-point ending-point) 

(let ((start (zero-vector 3)) 

(end (zero-vector 3)) 

(x-start nil) 

(y-start nil) 

(x-end nil) 

(y-end nil) 

(window-height nil) ) 
(setq window-height (send grasp-screen :send-pane 'grasp-window ': height ) ) 
(setq start (transform-points-global-frame view-frame starting-point) ) 
(setq end (transform-points-global-frame view-frame ending-point)) 
(setq x-start (round (+ x-origin-3d (* scale-3d (aref start 0))))) 

(setq y-start (round (+ window-height (* -1 (+ y-origin-3d (* scale-3d (aref start 2! 
))))))) 

(setq x-end (round (+ x-origin-3d (* scale-3d (aref end 0))))) 

(setq y-end (round (- window-height (+ y-origin-3d (* scale-3d (aref end 2)))))) 

(send grasp-screen :send-pane 'grasp-window ' : draw-line x-start y-start x-end y-end) ! 

1.4.3.3.3 SPHERE-3D-GRASP-WINDOW 
(defun sphere-3d-grasp-window (sphere-center radius) 

(let ((center (zero-vector 3)) 
(x-center) 
(y-center) 



)) 



(integer-radius) 

(window-height nil) ) 
(setq window-height (send grasp-screen :send-pane 'grasp-window ':height)) 
(setq center (transform-points-global-frame view-frame sphere-center) ) 
(setq x-center (round (+ x-origin-3d (* scale-3d (aref center 0))))) 
(setq y-center (round (- window-height (+ y-origin-3d (* scale-3d (aref center 2))))! 



(setq integer-radius (round (* scale-3d radius))) 

(send grasp-screen :send-pane 'grasp-window ': draw-filled- in-circle x-center y-cente! 
r integer-radius tv:alu-ior) ) ) 



1.4.3.3.4 DRAW-COORDINATES 
(defun draw-coordinates (frames length) 

(cond ( (listp frames) (draw-coordinate-list frames length) ) 
(t (draw-coordinate frames length) ) ) ) 

1.4. 3. 3. 4. A DRAW-COORDINATES Auxiliary 
(defun draw-coordinate-list (frames length) 
(cond ( (null frames) nil) 

(t (draw-coordinate (car frames) length) 

(draw-coordinate-list (cdr frames) length)))) 

(defun draw-coordinate (frame length) 

(let* ( (o-f rame (make-array '(3) : initial-contents (list 0))) 

(x-frame (make-array '(3) : initial-contents (list length 0))) 
(y-frame (make-array '(3) : initial-contents (list length 0))) 
(z-frame (make-array '(3) : initial-contents (list length))) 
(origin-3d (transform-points-frame-global frame o-f rame) ) 
(x (transform-points-frame-global frame x-frame) ) 
(y (transform-points-frame-global frame y-frame) ) 
(z (transform-points-frame-global frame z-frame))) 
(draw-3d-grasp-window origin-3d x) 
(draw-3d-grasp-window origin-3d y) 
(draw-3d-grasp-window origin-3d z) ) 
(draw-coordinate-labels frame length) ) 



(defun draw 
(let* ((x 

(x 

(x 

(x 

(y 
(y 
(y 
(y 

(z- 

(z- 

(z- 

(z- 

(setq x- 
(setq x- 
(setq x- 
(setq x- 
(setq y- 



-coordinate-labels (frame 

' (3) 
(list 
' (3) 
(list 



-point-1 (make-array 
point-2 (make-array 



-point-1 (make-array 



•point-3 (make-array ' (3) 

(list 

point-4 (make-array ' (3) 

(list 
' (3) 
(list 

-point-2 (make-array ' (3) 

(list 

■point-3 (make-array ' (3) 

(list 

■point-4 (make-array ' (3) 

(list 

•point-1 (make-array ' (3) 

(list 
(make-array ' (3) 
(list 
' (3) 
(list 
(make-array ' (3) 

(list 
( transform-point s - 
( transform-point s- 

•point-3 (transform-points- 

•point-4 (transform-points- 

■point-1 (transform-points- 



(* length -0.05)))) 
(* length 0.05) )) ) 



-point-2 

-point-3 

-point-4 

•point-1 
•point-2 



(make-array 



length) 

: initial-contents 

(* length 1.1) 
: initial-contents 

(* length 1.17) 
: initial-contents 

(* length 1.1) (* length 0.05)))) 
: initial-contents 

(* length 1.17) (* length -0.05)))) 
: initial-contents 

(* length 1.1) (* length 0.05)))) 
: initial-contents 

(* length 1.135) 0) ) ) 
: initial-contents 

(* length 1.17) 
: initial-contents 

(* length 1.135) 
: initial-contents 

(* length -0.035) 
: initial-contents 

(* length 0.035) 
: initial-contents 

(* length -0.035) 
: initial-contents 

(* length 0.035) (* length 1 
-frame-global frame x-point-1) ) 
-frame-global frame x-point-2) ) 
-frame-global frame x-point-3) ) 
-frame-global frame x-point-4)) 
-frame-global frame y-point-1) ) 



(* length 0.05) )) ) 

(* length -0.05) )) ) 
(* length 1.15)) ) ) 
(* length 1.15)))) 
(* length 1.1) ) )) 
1))))) 



(setq 
(setq 
(setq 
(setq 
(setq 
(setq 
(setq 
(draw- 
(draw- 
(draw- 
(draw- 
(draw- 
(draw- 
(draw- 
(draw- 



y-point- 

y-point- 

y-point- 

z-point- 

z-point- 

z-point- 

z-point- 

-3d-grasp 

-3d-grasp 

-3d-grasp 

-3d-grasp 

-3d-grasp 

-3d-grasp 

-3d-grasp 

-3d-grasp 



(trans 
(trans 
(trans 
(trans 
(trans 
(trans 
(trans 
-window 
-window 
-window 
-window 
-window 
window 
-window 
-window 



form-points 

form-points 

form-points 

form-points 

form-points 

form-points 

form-points 

x-point-1 x 

x-point-3 

y-point-1 

y-point-2 

y-point-2 

z-point-1 

z-point-2 

z-point-3 



-f rame- 
-frame- 
-frame- 
-frame- 
-frame- 
-f rame- 
- frame - 
-point- 
-point- 
-point- 
-point- 
-point- 
-point- 
-point- 
-point- 



•global 

•global 

•global 

■global 

■global 

•global 

•global 

2) 

4) 

2) 

3) 

4) 

2) 

3) 

4))) 



frame y- 
frame y- 
frame y- 
frame z- 
frame z- 
frame z- 
frame z- 



■point-2) ) 
•point-3) ) 
•point-4) ) 
•point-1) ) 
•point-2) ) 
•point-3) ) 
•point-4) ) 



1.4.3.3.5 DRAW-3D-LIST 
(defun draw-3d-list (list-center list) 
(let ((center (zero-vector 3)) 
(x-center) 
(y-center) 
(window-height ) 
(element) 
(n-elements) 
(start) ) 
(setq window-height (send grasp-screen :send-pane 'grasp-window ':height)) 
(setq center (transform-points-global-frame view-frame list-center)) 
(setq x-center (round (+ x-origin-3d (* scale-3d (aref center 0))))) 
(setq y-center (round (- window-height (+ y-origin-3d (* scale-3d (aref center 2))))! 



)) 



(setq n-elements (count-atoms list)) 
(do ((i (+ i 1))) 

( (= i n-elements) ) 

(setq start (round (- x-center (* 10 (/ n-elements 2))))) 

(setq element (car list)) 

(draw-character (+ (* 10 i) start) y-center element) 

(setq list (cdr list))))) 



1.4.3.3.6 DRAW-CONTACTS -GRASP -SPACE 
(defun draw-contacts-grasp-space () 

(setq view-frame (construct-view-frame 0.4 0.4)) 
(let* ( (contact-frames-grasp-space 

(transform-frames-global-frame *grasp-frame* *contact-frames*) ) 
(grasp-frame-grasp-space (make-array '(3 4) : initial-contents ' ( (1 

(0 
(0 



)) 



(draw-coordinates (first contact-frames-grasp-space) 0.5) 

(draw-coordinates (second contact-frames-grasp-space) 0.5) 

(draw-coordinates (third contact-frames-grasp-space) 0.5) 

(draw-coordinates grasp-frame-grasp-space 1.0))) 



0) 
0) 
0))) ! 



1.4.3.3.7 DRAW-CHARACTER 
(defun draw-character (x y char) 

(cond ( (numberp char) (send grasp-screen :send-pane 'grasp-window 

': draw-string (+ 48 char) x y) ) 
(t (send grasp-screen :send-pane 'grasp-window ': draw-string char x y) ) ) ) 

;;;*************************************************^ 
***** 



2 . Sensed and global variables 



;;;*************************************************************************************! 
***** 



2 . Temporary variables used to construct globa ! 



; sensed variables for simulation 

; 2.0.1 Define temporary variables 

(defvar *original-contact -points-object-space*) 

(defvar *original-contact-normals-object-space*) 

(defvar *original-object-f rame*) 

(defvar *contact-points-object-space*) 

(defvar *contact-normals-object-space*) 

(defvar *object-f rame*) 

(defvar *hand-frame*) 

(defvar *grasp-f rame*) 



2.0.2 Set default temporary variables 
2.0.2.1 Functions to set default values 



(defun set-original-values () 

(setq *original-contact-points-object-space* 

(list (make-array '(3) : initial-contents '(0 1.3 -0.8)) 
(make-array '(3) : initial-contents '(0 1.3 0.8)) 
(make-array '(3) : initial-contents '(0 -1.3 0)))) 
(setq *original-contact-normals-object-space* 

(list (make-array '(3) : initial-contents '(0 1 0)) 
(make-array '(3) : initial-contents '(0 1 0)) 
(make-array '(3) : initial-contents '(0 -1 0))))) 

(set-original-values) 

' 2.0.2.2 Set temporary variables 

(setq *hand-frame* (make-array '(3 4) : initial-contents '((1 0) 

(0 10 0) 
(0 1 0)))) 
(setq *onginal-object-frame* (make-array '(3 4) : initial-contents '((0 

(1 

(0 10- 
(setq *contact -points-object-space* *original-contact -points-object-space*) 
(setq *contact-normals-object-space* *original-contact-normals-object-space*) 
(setq *object-frame* *original-object-f rame*) 



-0.8) 

2.7) 

-3.0)))) 



(defvar *contact-points*) 
(defvar *contact-normals*) 
(defvar *contact-frames*) 



2 . 1 Define sensed global variables 



(defun generate-grasp-frame () 

(let* ( (xl (aref (first *contact-f rames*) 3)) 

(first *contact-f rames*) 1 3) ) 

(first *contact-frames*) 2 3)) 

(second *contact-frames*) 3) ) 

(second *contact-f rames*) 1 3)) 

(second *contact-f rames*) 2 3) ) 



2.2 Set defualt initial global variables 

2.2.1 Functions to set initial global variables 



(yi 

(zl 
<x2 

<y2 

(z2 
(x3 

<y3 

(z3 



(aref 
(aref 
(aref 
(aref 
(aref 
(aref 
(aref 
(aref 



(setf 
(setf 
(setf 
(setf 
(setf 
(setf 



(third *contact-frames*) 
(third *contact-frames*) 
(third *contact-f rames*) 
(centroid (make-array ' (3) ) ) 
(normal (make-array '(3)))) 
(aref centroid 0) (/ (+ xl x2 x3) 
(aref centroid 1) (/ (+ yl y2 y3) 
(aref centroid 2) (/ (+ zl z2 z3) 
(aref normal 0) (- (* (- y3 yl) (■ 



(aref normal 1) (- (* 
(aref normal 2) (- (* 



(- z3 zl) (- 
(■ 



(generate-f rame centroid normal) ) ) 



3)) 

1 3)) 

2 3)) 






3)) 
3)) 
3)) 






z2 zl)) 
x2 xl) ) 
y2 yl)) 


(* 
(* 
(* 


(- z3 zl) (- y2 yl)))) 
(- x3 xl) (- z2 zl) ) ) ) 
(- y3 yl) (- x2 xl) ) ) ) 



(defun normalize-contact-normals (contact-normals) 
(cond ( (null contact-normals) nil) 

(t (cons (normalize-contact-normal (car contact-normals) ) 

(normalize-contact-normals (cdr contact-normals) ) ) ) ) ) 

(defun normalize-contact-normal (contact-normal) 

(let ( (magnitude (sqrt (+ (sqr (aref contact-normal 0) ) 

(sqr (aref contact -normal 1) ) 
(sqr (aref contact-normal 2)))))) 
(setf (aref contact-normal 0) (/ (aref contact-normal 0) magnitude)) 
(setf (aref contact-normal 1) (/ (aref contact-normal 1) magnitude)) 
(setf (aref contact-normal 2) (/ (aref contact-normal 2) magnitude)) 
contact-normal) ) 

(defun normalize-object-f rame (object-frame) 
(let ( (magnitude (zero-vector 3) ) ) 
(do (<i (+ i 1))) 
((= i 3)) 
(setf (aref magnitude i) (sqrt (+ (sqr (aref object-frame i) ) 

(sqr (aref object-frame 1 i) ) 
(sqr (aref object-frame 2 i) ) ) ) ) 
(do <(j <+ j 1)>) 
((= j 3)) 
(setf (aref object-frame j i) (/ (aref object-frame j i) (aref magnitude i) ) ) ) ) ) 
object -frame) 

; 2.2.2 Set global-variables 

(defun initialize-global-variables () 
(setq *contact-normals-object-space* 

(normalize-contact-normals *contact-normals-object-space*) ) 
(setq *object-f rame* (normalize-object-frame *object-f rame*) ) 
(setq *contact-points* (transform-points-frame-global *object-f rame* 

*contact-points-object-space*) ) 
(setq *contact-normals* (transform-direction-frame-global *object-frame* 

*contact -normals-object-space*) ) 
(setq *contact-f rames* (generate-f rame *contact-points* *contact-normals*) ) 
(setq *grasp-frame* (generate-grasp-f rame) ) ) 

(initialize-global-variables) 

...*************************************************************************************' 
***** 

I 

; 3. Constraint 

...•A***********************************************************************************! 
***** 

; 3.1 Define constraint variables 

(defvar *basis-wrenches*) 
(defvar *basis-wrench*) 
(defvar *virtual-work*) 
(defvar *contact-types*) 

; 3.2.1 Set defualt variable value section 

3.2.1.1 Functions for setting global variables 

(defun construct-basis-wrench (contact-frames) 
(cond ( (null contact-frames) nil) 

(t (cons (transform-basis-wrench *basis-wrench* (car contact-frames)) 
(construct-basis-wrench (cdr contact-frames) ) ) ) ) ) 

(defun transform-basis-wrench (basis-wrench contact-frame) 
(let* ( (transformed-basis-wrench 



(transform-direction-frame-global contact-frame basis-wrench) ) 
(x (aref contact-frame 3) ) 
(y (aref contact-frame 1 3) ) 
(z (aref contact-frame 2 3))) 
(do ((i (+ 1 i))) 

( (= i 11) transformed-basis-wrench) 
(setf (aref transformed-basis-wrench 3 i) 

(+ (* (aref transformed-basis-wrench 1 i) z -1) 
(* (aref transformed-basis-wrench 2 i) y) 
(aref transformed-basis-wrench 3 i) ) ) 
(setf (aref transformed-basis-wrench 4 i) 

(+ (* (aref transformed-basis-wrench 2 i) x -1) 
(* (aref transformed-basis-wrench i) z) 
(aref transformed-basis-wrench 4 i) ) ) 
(setf (aref transformed-basis-wrench 5 i) 

(+ (* (aref transformed-basis-wrench i) y -1) 
(* (aref transformed-basis-wrench 1 i) x) 
(aref transformed-basis-wrench 5 i) ) ) ) ) ) 

3.2.1.2 Set constraint variables 



(setq *contact -types* (list (make-array ' (12) 

' (1 1 

(make-array ' (12) 
' (1 1 

(make-array ' (12) 
' (0 

(make-array ' (12) 



: initial-contents 
101111000 1)) 
: initial-contents 
100011000 0)) 
: initial-contents 
100000000 0)) 
: initial-contents 



'(0 0000000000 0)))) 
(setq *basis-wrench* (make-array '(6 12) : initial-contents '((100000-1 



0) 



(0 100000-1000 0) 

(0 0100000-100 0) 

(0 00100000-10 0) 

(0 000100000-10) 

(0 0000100000-1)))! 



(setq *basis-wrenches* (construct-basis-wrench *contact-frames*) ) 



' 3.3 Constraint functions 

3.3.1 CONSTRUCT-VIRTUAL-WORK-LIST 
(defun construct-virtual-work-list (basis-wrenches twist) 
(cond ( (null basis-wrenches) nil) 

(t (cons (construct-virtual-work (car basis-wrenches) twist) 

(construct-virtual-work-list (cdr basis-wrenches) twist))))) 

3. 3.1. A CONSTRUCT-VIRTUAL-WORK-LIST Auxiliary 

(defun construct-virtual-work (basis-wrench twist) 
(let ((virtual-work-vector (make-array '(12)))) 
(do ((i (+ 1 i))) 

( (= i 12) virtual-work-vector) 
(setf (aref virtual-work-vector i) 

(virtual-work (get-array-column basis-wrench i) twist) ) ) ) ) 

(defun virtual-work (wrench twist) 

(+ (* (aref wrench 0) (aref twist 3)) 

(aref wrench 1) (aref twist 4)) 

(aref wrench 2) (aref twist 5)) 

(aref wrench 3) (aref twist 0) ) 

(aref wrench 4) (aref twist 1)) 

(aref wrench 5) (aref twist 2)))) 



(* 
(* 
(* 
(* 
(* 



3.3.2 DETERMINE-CONTACT-TYPES 



(defun determine-contact-types (virtual-work-pattern-list) 
(cond ( (null virtual-work-pattern-list) nil) 

(t (cons (determine-contact-type (car virtual-work-pattern-list) *contact-types* ! 
) 

(determine-contact-types (cdr virtual-work-pattern-list)))))) 

3. 3. 2. A DETERMINE-CONTACT-TYPES Auxiliary 

(defun determine-contact-type (virtual-work-pattern contact-types) 
(cond ( (null contact-types) nil) 

(t (cons (test-slip virtual-work-pattern (car contact-types) ) 

(determine-contact-type virtual-work-pattern (cdr contact-types) ) ) ) ) ) 

(defun test-slip (virtual-work-pattern contact-type-pattern) 
(let ((total 0)) 

(do ((i (+ 1 i))) 

( (= i 12) (= total 0)) 
(setq total (+ total (* (aref virtual-work-pattern i) 

(aref contact-type-pattern i) ) ) ) ) ) ) 

(defun construct-virtual-work-pattern-list (virtual-work-list) 
(cond ( (null virtual-work-list) nil) 

(t (cons (construct -virtual-work-pattern (car virtual-work-list) ) 

(construct-virtual-work-pattern-list (cdr virtual-work-list) ) ) ) ) ) 

(defun construct-virtual-work-pattern (virtual-work) 

(let ((virtual-work-pattern-vector (make-array '(12)))) 
(do ((i (+ 1 i))) 

( (= i 12) virtual-work-pattern-vector) 
(cond ( (>= (aref virtual-work i) 0) (setf (aref virtual-work-pattern-vector i) 0)) 
(t (setf (aref virtual-work-pattern-vector i) 1) ) ) ) ) ) 

(defun constraint-state (twist twist-ref) 

(constraint-state-aux (constraint twist twist-ref) ) ) 

(defun constraint-state-aux (states) 
(cond ( (null states) nil) 

(t (cons (constraint-state-aux-1 (car states) ) 

(constraint-state-aux (cdr states) ) ) ) ) ) 

(defun constraint-state-aux-1 (state) 
(cond ( (car state) 1) 

( (+ 1 (constraint-state-aux-1 (cdr state) ) ) ) ) ) 

(defun constraint (twist twist-ref) 

(let ( (twist-origin (twist-org twist twist-ref) ) ) 
(determine-contact-types 

(construct -virtual-work-pattern-list 

(construct -virtual-work-list *basis-wrenches* twist-origin) ) ) ) ) 

(defun twist-org (twist twist-ref) 
(let ((twist-org (make-array '(6))) 
(rx (* -1 (aref twist-ref 0))) 
(ry (* -1 (aref twist-ref 1))) 
(rz (* -1 (aref twist-ref 2)))) 
(setf (aref twist-org 0) (aref twist 0)) 
(setf (aref twist-org 1) (aref twist 1)) 
(setf (aref twist-org 2) (aref twist 2)) 
(setf (aref twist-org 3) (+ (aref twist 2) 

(* (aref twist 1) rz) 
(* (aref twist 2) ry -1))) 
(setf (aref twist-org 4) (+ (aref twist 4) 

(* (aref twist 2) rx) 

(* (aref twist 0) rz -1)) ) 



(setf (aref twist-org 5) (+ (aref twist 5) 

(* (aref twist 0) ry) 
(* (aref twist 1) rx -1))) 
twist-org) ) 

; 3.4 Geometric Constraint 

3.4.1 DRAW-CONTACT-TRAJECTORY 
(defun draw-contact-trajectory (contact-point twist twist-ref twist -magnitude steps) 
(let ((new-contact-point (zero-vector 3))) 
(do ((i 1 (+ i 1))) 
( (= i steps) ) 
(setq new-contact-point 

(determine-contact-point contact -point twist twist-ref 

(/ (* i twist-magnitude) steps))) 
(draw-3d-grasp-window contact -point new-contact-point) 
(setq contact-point new-contact -point) )) ) 



3. 4.1. A DRAW-CONTACT-TRAJECTORY Auxiliary 
(defun determine-contact-point (contact -point twist twist-ref twist -magnitude) 
(let* ( (rotation-matrix (zero-array 3 3) ) 
(twist-radius (zero-vector 3) ) 
(translation (zero-vector 3)) 
(new-contact-point (zero-vector 3)) 
(tl (aref twist 0) ) 
(t2 (aref twist 1) ) 
(t3 (aref twist 2) ) 
(t4 (aref twist 3) ) 
(t5 (aref twist 4) ) 
(t6 (aref twist 5) ) 
(en (cos twist -magnitude) ) 
(sn (sin twist-magnitude) ) ) 



(setf (aref rotation-matrix 

(setf (aref rotation-matrix 

(setf (aref rotation-matrix 

(setf (aref rotation-matrix 1 

(setf (aref rotation-matrix 1 

(setf (aref rotation-matrix 1 

(setf (aref rotation-matrix 2 

(setf (aref rotation-matrix 2 

(setf (aref rotation-matrix 

(setf (aref twist-radius 0) 

(setf (aref twist-radius 1) 

(setf (aref twist-radius 2) 

(setf (aref translation 

(setf (aref translation 

(setf (aref translation 



0) 


( + 


(* 


tl 


tl 


(- 


1 


en) ) 


1) 


(- 


(* 


t2 


tl 


(- 


1 


en) ) 


2) 


( + 


(* 


t3 


tl 


(- 


1 


en) ) 


0) 


( + 


(* 


tl 


t2 


(- 


1 


en) ) 


1) 


( + 


(* 


t2 


t2 


(- 


1 


en) ) 


2) 


(- 


(* 


t3 


t2 


(- 


1 


en) ) 


0) 


(- 


(* 


tl 


t3 


<- 


1 


en) ) 


1) 


( + 


(* 


t2 


t3 


(- 


1 


en) ) 


2) 


( + 


(* 


t3 


t3 


(- 


1 


en) ) 



en)) 

(* t3 sn))) 

(* t2 sn))) 

(* t3 sn))) 

en)) 

(* tl sn))) 

(* t2 sn))) 

(* tl sn))) 

en)) 

(aref twist-ref 0) ) ) 
(aref twist-ref 1) ) ) 
(aref twist-ref 2))) 



(setq new-contact-point 



2 

(- (aref contact-point 0) 
(- (aref contact-point 1) 
(- (aref contact-point 2) 

0) (* twist-magnitude t4) ) 

1) (* twist-magnitude t5) ) 

2) (* twist-magnitude t6) ) 
(add-array-list (list 

(multiply-array-list (list rotation-matrix twist-radius) ) 
twist-ref translation) ) ) ) ) 



;; , •*************************************************************************************! 
***** 



4 . Body wrench 



;;*************************************************************************************) 

***** 



(defvar *body-wrench*) 
(defvar *object-mass*) 
(defvar *gravitational-acceleration*) 



4 . 1 Define body wrench variables 



(defun construct-body-wrench () 



4.2 Set body wrench variables section 
4.2.1 Auxiliary body wrench functions 



(let ((body-wrench (zero-vector 6)) 

(weight (* *object-mass* *gravitational-acceleration*) ) ) 
(setf (aref body-wrench 2) weight) 

(setf (aref body-wrench 3) (* (aref *object-f rame* 1 3) weight)) 
(setf (aref body-wrench 4) (* -1 (aref *object-f rame* 3) weight)) 



body- wrench) ) 



4.2.2 Set body wrench variables 



(setq *object-mass* 0) 

(setq *gravitational-acceleration* 32.2) 

(setq *body-wrench* (construct-body-wrench) ) 

'" 4.3 Body wrench functions 

4.3.1 ORIENTATION 
(defun orientation (twist-cg) 

(let* ( (t3 (aref twist-cg 3)) 
(t4 (aref twist-cg 4) ) 
(t5 (aref twist-cg 5) ) 

(twist-magnitude (sqrt (abs (+ (* t3 t3) (* t4 t4) (* t5 t5) ) ) ) ) 
(theta (asin (/ t3 twist-magnitude))) 
(phi (asin (/ (* -1 t4) twist-magnitude)))) 
(list theta phi))) 

4.3.2 TWIST-CG 
(defun twist-cg (twist twist-ref eg) 

(let ((twist-cg (make-array '(6))) 

(rx (- (aref eg 0) (aref twist-ref 0))) 
(ry (- (aref eg 1) (aref twist-ref 1))) 
(rz (- (aref eg 2) (aref twist-ref 2)))) 
(setf (aref twist-cg 0) (aref twist 0)) 
(setf (aref twist-cg 1) (aref twist 1)) 
(setf (aref twist-cg 2) (aref twist 2)) 
(setf (aref twist-cg 3) (+ (aref twist 2) 

(* (aref twist 1) rz) 
(* (aref twist 2) ry -1))) 
(setf (aref twist-cg 4) (+ (aref twist 4) 

(* (aref twist 2) rx) 
(* (aref twist 0) rz -1))) 
(setf (aref twist-cg 5) (+ (aref twist 5) 

(* (aref twist 0) ry) 
(* (aref twist 1) rx -1))) 
twist-cg) ) 

;;;********************************************* ****************************************, 

***** 

5 . Contact Wrenches 

r 

***** 

; 5.1 Define contact wrench variables 

(defvar *f ingertip-stiffness*) 
(defvar *finger-stiffness*) 

(defvar *contact-stif fness*) 

(defvar *contact-stif fness-contact-f rame*) 

(defvar *contact-stif fness-hand-f rame*) 

(defvar *grasp-stiffness*) 
(defvar *grasp-compliance*) 

(defvar *contact-wrench*) 

(defvar *contact-wrench-contact-frame*) 

; 5.2 Set contact wrench variables section 



; 5.2.1 Functions to set contact wrench variables 

(defun construct-contact-wrench-variables () 

(setq *contact-stif fness* (construct-contact-stif fness *fingertip-stif fness* 

*finger-stiffness* 
*contact-frames*) ) 
(setq *contact -stiffness-contact-frame* (construct-contact-stif fness-cont act-frame 

*contact-stif fness* 
*contact-f rames*) ) 
(setq *contact-stiffness-hand-f rame* (construct-contact-stif fness-hand-frame 

*contact-st if fness* 
*contact-f rames*) ) 
(setq *grasp-stiffness* (add-array-list *contact-st if fness-hand-frame*) ) 
(setq *grasp-compliance* (math: invert -matrix *grasp-stiffness*) ) 
(setq *contact-wrench* (construct-contact-wrenches *contact-stiffness-contact-f rame* 

*of fset-wrench*) ) 
(setq *contact-wrench-cont act -frame* 

(construct-contact-wrenches-contact-frame *contact-wrench* *contact-frames*) ) ) 

; 5.2.1.1 Contact stiffness 

(defun construct-contact-stiffness (fingertip-stiffness finger-stiffness contact-frames) 
(cond ( (null contact-frames) nil) 

(t (cons (construct-one-contact-stif fness (car fingertip-stiffness) 

(car finger-stiffness) 
(car contact-frames)) 
(construct-contact-stiffness (cdr fingertip-stiffness) 

(cdr finger-stiffness) 
(cdr contact-frames) ) ) ) ) ) 

(defun construct-one-contact-stif fness (fingertip-stiffness finger-stiffness contact-fra! 
me) 

(cond ( (null finger-stiffness) (abs-array (transform-direction-frame-global 

contact-frame fingertip-stiffness) ) ) 
(t (add-stiffness finger-stiffness 

(abs-array (transform-direction-frame-global 

contact-frame fingertip-stiffness) ) ) ) ) ) 

(defun add-stiffness (stiffness-1 stiffness-2) 

(let* ((stiffness-dimension (array-dimension stiffness-1 0)) 
(new-stiffness (zero-vector stiffness-dimension))) 
(do <(i (+ i 1))) 

( (= i stiffness-dimension) new-stiffness) 
(setf (aref new-stiffness i) (/ (* (aref stiffness-1 i) (aref stiffness-2 i) ) 

(+ (aref stiffness-1 i) (aref stiffness-2 i) ) ) ) ) ) ) 



> 5.2.1.2 Contact stiffness contact frame 

(defun construct-contact-stiffness-contact-frame (contact-stiffness contact-frames) 
(cond ( (null contact-frames) nil) 

(t (cons (construct-one-contact-stiffness-contact-frame (car contact-stiffness) 

(car contact-frames) ) 
(const ruct-contact-st if fness-contact-f rame (cdr contact-stiffness) 

(cdr contact-frames) ) ) ) ) ) 

(defun construct-one-contact-stiffness-contact-frame (contact-stiffness contact-frame) 
(let ( (rx (aref contact-frame 3)) 
(ry (aref contact-frame 1 3) ) 
(rz (aref contact-frame 2 3) ) 
(kx (aref contact-stiffness 0)) 
(ky (aref contact-stiffness 1)) 
(kz (aref contact-stiffness 2)) 
(ktx (aref contact-stiffness 3) ) 
(kty (aref contact-stiffness 4) ) 
(ktz (aref contact-stiffness 5) ) 

(contact-stiffness-contact-frame (zero-array 6 6))) 
(setf (aref contact-stiffness-contact-frame 1) (* kx rz) ) 



(* -1 kx ry) ) 
kx) 

(* -1 ky rz)) 

(* ky rx)) 
ky) 

(* kz ry)) 

kz rx) ) 



(setf (aref contact-stiffness-contact-frame 2) 
(setf (aref contact-stiffness-contact-frame 3) 
(setf (aref contact-stiffness-contact-frame 1 0) 
(setf (aref contact-stiffness-contact-frame 1 2) 
(setf (aref contact-stiffness-contact-frame 1 4) 
(setf (aref contact-stiffness-contact-frame 2 0) 
(setf (aref contact-stiffness-contact-frame 2 1) 
(setf (aref contact-stiffness-contact-frame 2 5) 
(setf (aref contact-stiffness-contact-frame 3 0) 
(setf (aref contact-stiffness-contact-frame 4 1) 
(setf (aref contact-stiffness-contact-frame 5 2) 
contact-stiffness-contact-frame) ) 

5.2.1.3 Contact stiffness hand frame 
(defun construct-contact-stiffness-hand-frame (contact-stiffness contact-frames) 
(cond ( (null contact-frames) nil) 

(t (cons (construct-one-contact-stiffness-hand-frame (car contact-stiffness) 

(car contact -frames) ) 
(construct-contact-stiffness-hand-frame (cdr contact-stiffness) 

(cdr contact-frames) ) ) ) ) ) 



(* -: 

kz) 

ktx) 

kty) 

ktz) 



(defun const ruct-one-contact-st if fness-hand-frame 
(let ((rx (aref contact-frame 3)) 
(ry (aref contact-frame 1 3) ) 
(rz (aref contact-frame 2 3) ) 
(kx (aref contact-stiffness 0)) 
(ky (aref contact-stiffness 1) ) 
(kz (aref contact-stiffness 2)) 
(ktx (aref contact-stiffness 3) ) 
(kty (aref contact-stiffness 4)) 
(ktz (aref contact-stiffness 5) ) 
(contact-stiffness-hand-frame (zero-array 



(contact-stiffness contact-frame) 



(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 
(setf 



aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 4 
aref contact-stiffness-hand-frame 4 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 
aref contact-stiffness-hand-frame 



6 6))) 

(* kx rz) ) 

(* -1 kx ry) ) 

kx) 

(* -1 ky rz)) 

(* ky rx)) 

ky) 

(* kz ry)) 

(* -1 kz rx) ) 

kz) 

(+ (* ky rz rz) 

-1 kz rx ry) ) 

-1 ky rx rz) ) 

-1 ky rz) ) 

kz ry) ) 

-1 kz rx ry) ) 

(* kx rz rz) 

-1 kx ry rz) ) 

kx rz) ) 

-1 kz rx) ) 

-1 ky rx rz) ) 

-1 kx ry rz) ) 

(* kx ry ry) 

-1 kx ry) ) 

ky rx) ) 



(* kz ry ry) ktx) ) 



(* 
(* 
(* 
(* 
(* 
( + 
(* 
(* 
(* 
(* 
(* 
( + 
(* 
(* 



(* kz rx rx) kty) ) 



(* ky rx rx) ktz) ) 



contact-stiffness-hand-frame) ) 



5.2.1.3 Grasp stiffness 

5.2.1.4 Grasp compliance 

5.2.1.5 Contact wrench 

(defun construct-contact-wrenches (contact-stiffness-contact-frame offset-wrench) 
(cond ( (null contact-stiffness-contact-frame) nil) 

(t (cons (construct-contact-wrench (car contact-stiffness-contact-frame) 

(car of f set -wrench) ) 
(construct-contact-wrenches (cdr contact-stiffness-contact-frame) 

(cdr offset-wrench) ) ) ) ) ) 



(defun construct-contact-wrench (contact-stiffness-contact-frame offset-wrench) 
(add-array-list (list (multiply-array-list 

(list 

(multiply-array-list 

(list contact-stiffness-contact-frame *grasp-compliance*) ) 
*body-wrench*) ) 
offset-wrench) ) ) 

; 5.2.1.5 Contact wrench contact frame 

(defun construct-contact-wrenches-contact-frame (contact-wrenches contact-frames) 
(cond ( (null contact-wrenches) nil) 

(t (cons (transform-direction-global-frame (car contact-frames) 

(car contact-wrenches) ) 
(construct -contact -wrenches-contact-frame (cdr contact -wrenches) 

(cdr contact-frames)))))) 

; 5.2.2 Set contact wrench variables 

(setq *fingertip-stiffness* (list (make-array '(6) : initial-contents '(10 10 25 6) ) 

(make-array '(6) : initial-contents '(10 10 25 6) ) 

(make-array '(6) : initial-contents '(10 10 25 6)))! 
) 

(setq *finger-stiffness* (list nil nil nil)) 

(const ruct-contact-wrench-variables) 

;;;*************************************************************************************! 

***** 

6 . Contact -types 



; 6.1 Define contact type variables 

(defvar *f ric-coeff *) 
(defvar *mom-coeff*) 

/" 6.2 Set contact type variables section 

(setq *f ric-coeff* 0.6) 
(setq *mom-coeff* 0.1) 

; 6.3 Contact type functions 

6.3.1 CONTACT-TYPE 
(defun contact-type (contact -wrenches) 
(cond ((null contact-wrenches) nil) 

(t (cons (contact-type-aux (car contact -wrenches) ) 
(contact-type (cdr contact-wrenches) ) ) ) ) ) 

; 6. 3.1. A Auxiliary contact type functions 

(defun contact-type-aux (contact-wrench) 

(let* ((normal-force (aref contact-wrench 2)) 

(tangent-force (sqrt (+ (* (aref contact -wrench 0) (aref contact-wrench 0)) 

(* (aref contact-wrench 1) (aref contact-wrench 1))))) 
(moment (abs (aref contact-wrench 5))) 

(max-tangent-force (* *fric-coeff* (abs normal-force))) 
(max-moment (* *mom-coeff* (abs normal-force)))) 
(cond ( (>= normal-force 0) 4) 

( (>= tangent-force max-tangent-force) 3) 
( (>= moment max-moment) 2) 
(t 1)))) 

;;;*************************************************************************************i 
**** 



Iff 

***** 



7 . Offset wrench 

*************************************************************************************! 



; 7.1 Define offset wrench variables 

(defvar *off set-wrench*) 

(defvar *off set-wrench-contact-frame*) 

(defvar *scale-force*) 

; 7.2 Set offset wrench defaults 

(setq *offset-wrench* (list (zero-vector 6) (zero-vector 6) (zero-vector 6))) 
(setq *off set-wrench-contact-frame* (zero-vector 6) ) 
(setq *scale-force* 1.0) 

; 7.3 Offset wrench functions 

7.3.1 CONSTRUCT-OFFSET-WRENCHES 
(defun construct-off set-wrenches (force-center force-magnitude) 

(setq *off set-wrench* (offset-wrench-three-contacts force-center force-magnitude) ) 
(setq *off set-wrench-contact-frame* 

(transform-direction-global-frame *contact-frames* *of fset-wrench*) ) ) 

7. 3.1, A OFFSET-WRENCH-THREE-CONTACTS 

; There are two solutions for the forces. One solution in which most of the force vector! 
s are 

; directed into the force-centroid and other in which two or more force vectors are 
; directed away from the centroid. 

(defun offset-wrench-three-contacts (force-center force-magnitude) 
(let* ((force-array (zero-array 3 3)) 

(reduced-force-array (make-array ' (3 3) ) ) 

(offsetl (zero-vector 6)) 

(offset2 (zero-vector 6)) 

(offset3 (zero-vector 6)) 

(length-1) (length-2) (length-3) 

(force-1) (force-2) (force-3) 

(x (aref force-center 0)) 

(y (aref force-center 1)) 

(z (aref force-center 2)) 

(a) (b)) 



(setq length-1 (sqrt (+ 



(setq length-2 (sqrt (+ 



(setq length-3 (sqrt (+ 



(setf (aref force-array 

(setf (aref force-array 

(setf (aref force-array 

(setf (aref force-array 1 

(setf (aref force-array 1 



(first *contact-frames*) 3))) 
(first *contact-frames*) 13))) 
(first *contact-f rames*) 2 3) ) ) ) ) ) 
(second *contact-frames*) 3))) 
(second *contact-f rames*) 13))) 
(second *contact-f rames*) 2 3) ) ) ) ) ) 
(third *contact-f rames*) 3))) 
(third *contact-f rames*) 13))) 
(third *contact-frames*) 2 3)))))) 

0) (/ (- x (aref (first *contact-f rames*) 3)) length-1)) 

1) (/ (- x (aref (second *contact-f rames*) 3)) length-2)! 

2) (/ (- x (aref (third *contact-frames*) 3)) length-3)) 

0) (/ (- y (aref (first *contact-frames*) 1 3)) length-1)) 

1) (/ (- y (aref (second *contact-frames*) 1 3)) length-2)! 



(sqr 


(- 


X 


(aref 


(sqr 


(- 


y 


(aref 


(sqr 


(- 


z 


(aref 


(sqr 


(- 


X 


(aref 


(sqr 


(- 


y 


(aref 


(sqr 


(- 


z 


(aref 


(sqr 


(- 


X 


(aref 


(sqr 


(- 


y 


(aref 


(sqr 


(- 


z 


(aref 



(setf (aref force-array 1 2) (/ (- y (aref (third *contact-frames*) 1 3)) length-3))! 



(setf (aref force-array 2 
(setf (aref force-array 2 



(setf (aref force-array 2 
(if (< force-magnitude 0) 



y)))) 



0) (/ (- z (aref (first *contact-f rames*) 2 3)) length-1)) 

1) (/ (- z (aref (second *contact-frames*) 2 3)) length-2)! 

2) (/ (- z (aref (third *contact-frames*) 2 3)) length-3)) 
(setq force-array (multiply-array-list (list -1 force-arra! 



(setq reduced-force-array (row-reduced-echelon force-array) ) 

(setq a (aref reduced-force-array 2) ) 

(setq b (aref reduced-force-array 1 2)) 

(setq force-3 (sqrt (/ (sqr force-magnitude) (+ (sqr a) (sqr b) 1) ) ) ) 

(setq force-2 (* -1 b force-3)) 

(setq force-1 (* -1 a force-3)) 



(setf 


(aref 


offsetl 


0) 


(* 


*scale-force* 


force-1 


(aref 


force-array 





0))) 


(setf 


(aref 


offsetl 


1) 


(* 


*scale-force* 


force-1 


(aref 


force-array 


1 


0))) 


(setf 


(aref 


offsetl 


2) 


(* 


* scale-force* 


force-1 


(aref 


force-array 


2 


0))) 


(setf 


(aref 


offset2 


0) 


(* 


*scale-force* 


force-2 


(aref 


force-array 





1))) 


(setf 


(aref 


offset2 


1) 


(* 


*scale-force* 


force-2 


(aref 


force-array 


1 


1))) 


(setf 


(aref 


offset2 


2) 


(* 


*scale-force* 


force-2 


(aref 


force-array 


2 


1))) 


(setf 


(aref 


of f set3 


0) 


(* 


*scale-force* 


force-3 


(aref 


force-array 





2))) 


(setf 


(aref 


of fset3 


1) 


(* 


*scale-force* 


force-3 


(aref 


force-array 


1 


2))) 


(setf 


(aref 


offset3 


2) 


( * 


*scale-force* 


force-3 


(aref 


force-array 


2 


2))) 


(list 


offsetl offset2 off set3) ) ) 












...************************************************************************* 
/ / / 


***** 























8. Controlled Slip 
.*************************************************************************************! 



***** 



8.1 DETERMINE-CONSTRAINT-STATE 
(defun determine-constraint-state (force-center force -magnitude) 

(let ( (grasp-vector-list) 

(constraint-state) 

(label-point 

(transform-points-global-frame *grasp-f rame* force-center) ) ) 
(construct-off set-wrenches force-center force-magnitude) 

(setq grasp-vector-list (construct-grasp-points force-center force-magnitude) ) 
( const ruct-cont act -wrench-variables) 

(setq constraint-state (contact-type *contact-wrench-contact-frame*) ) 
(draw-coordinate-system) 

(sphere-3d-grasp-window (first grasp-vector-list) 0.01) 
(sphere-3d-grasp-window (second grasp-vector-list) 0.01) 
(sphere-3d-grasp-window (third grasp-vector-list) 0.01) 
(sphere-3d-grasp-window force-center 0.02) 

(setf (aref label-point 1) (+ (aref label-point 1) 0.025)) 

(setq label-point (transform-points-frame-global *grasp-frame* label-point) ) 
(draw-3d-list label-point constraint-state) ) ) 

8.2 MAP-CONSTRAINT-SPACE 



(defun map-constraint-state 

(x-start x-end x-inc y-start y-end y-inc force-magnitude) 
(let ( (force-center-grasp-frame (zero-vector 3) ) 
(force-center-hand-frame (zero-vector 3) ) 
(label-point) 
(constraint-state) ) 
(do ( (x x-start (+ x x-inc) ) ) 
( (>= x x-end) ) 
(do ( (y y-start (+ y y-inc) ) ) 
( (>= y y-end) ) 
(setf (aref force-center-grasp-frame 0) x) 
(setf (aref force-center-grasp-frame 1) y) 
(setq label-point force-center-grasp-frame) 
(setq force-center-hand-frame 

(transform-points-frame-global *grasp-frame* force-center-grasp-frame) ) 
(construct -offset-wrenches force-center-hand-frame force-magnitude) 
(construct -contact -wrench-variables) 



(setq constraint-state (contact-type *contact-wrench-contact-f rame*) ) 

(sphere-3d-grasp-window force-center-hand-frame 0.02) 

(setf (aref label-point 1) (+ (aref label-point 1) 0.025)) 

(setq label-point (transform-points-frame-global *grasp-frame* label-point)) 

(draw-3d-list label-point constraint-state) ) ) ) ) 



(defun map-constraint-space (force-magnitude) 
(let* ( (point -grasp-frame (make-array '(3) : initial-contents '(0 0))) 
(point -hand-frame (make-array '(3) : initial-contents '(0 0))) 
(constraint-state) ) 
(do <(i -1 (+ i 0.2))) 
((>= i D) 
(do <(j -1 (+ j 0.2))) 
((>= j D) 
(setf (aref point -grasp-frame 0) i) 
(setf (aref point -grasp-frame 1) j) 
(setq point -hand-frame (transform-points-frame-global *grasp-frame* point -grasp-f! 



rame) ) 
de)) 

rame) ) 



(setq constraint-state (determine-constraint-state point -hand-frame f orce-magnitu ! 

(sphere-3d-grasp-window point -hand-frame 0.01) 

(setf (aref point -grasp-f rame 1) (+ j 0.01)) 

(setq point-hand-frame (transform-points-frame-global *grasp-frame* point -grasp-f ! 

(draw-3d-list point -hand-frame constraint-state) ) ) ) ) 



***** 

t 

; 9. Hand actuation routines 

/ 
;;; *****************************y r ******^ A * A * yt ^^ AA ^^^^^^^^^ AA ^^^ AA ^^^^^ A ^^^^ A ^^ A ^^^^ A ^^ A ^ | 

**** 



' 9.1 Define actuation constants and variables 

(defvar *default-tra jectory-gen* (if (boundp 'tg) tg) ) 
(defvar *default-parallel-conn* (if (boundp 'pc) pc) ) 
(defvar *fingertip-radius*) 
(defvar *test-point*) 
(defvar *scale-force*) 

' 9.3 Set actuation variables 

(setq *fingertip-radius* 0.408) 

(setq *test-point* (make-array '(3) : initial-contents '(0.0 2.5 0.0))) 

(setq *scale-force* 1.0) 

(setq *offset-wrench* (list (zero-vector 6) (zero-vector 6) (zero-vector 6))) 

; 9.4 Grasp functions 

9.4.1 MOVE-TO-CONTACT-POINTS 

(defun move-to-contact-points () 

(let* ((fingertip-vector (construct-fingertip-vector 

(determine-fingertip-centers *contact-points* *contact-f ram! 
es*))) 

(dur 0.4) ) 

(if (send tg :move-fingers-to fingertip-vector : duration dur) 

(send tg :send-traj pc) ) ) ) 

9.4.2 MOVE-TO-GRASP -CENTER 



(defun move-to-grasp-center (force-center force-magnitude) 

(let* ((force-center-hand-frame (transform-points-frame-global *grasp-f rame* force-cen! 
ter)) 

(magnitude (* *scale-force* force-magnitude)) 

(grasp-vector-list (construct-grasp-points force-center-hand-frame magnitude) ) 

(fingertip-center-list 

(determine-fingertip-centers grasp-vector-list *contact-f rames*) ) 
(fingertip-vector (construct-f ingertip-vector fingertip-center-list) ) 
(dur 0.4)) 
(construct-off set-wrenches force-center force-magnitude) 
(clearscreen) 
(draw-coordinate-system) 

(sphere-3d-grasp-window (first grasp-vector-list) 0.01) 
(sphere-3d-grasp-window (second grasp-vector-list) 0.01) 
(sphere-3d-grasp-window (third grasp-vector-list) 0.01) 
(sphere-3d-grasp-window force-center-hand-frame 0.02) 
(if (send tg :move-fingers-to fingertip-vector :duration dur) 
(send tg :send-traj pc) ) ) ) 



9.5 Auxiliary functions 

9.6.1 MOVE-TO-CONTACT-POINTS auxiliary function! 



(defun determine-fingertip-centers (contact-points contact-frames) 
(cond ( (null contact-frames) nil) 

(t (cons (determine-fingertip-center (car contact-points) (car contact-frames)) 

(determine-fingertip-centers (cdr contact-points) (cdr contact-frames)))! 
))) 

(defun determine-fingertip-center (contact-point contact-frame) 
(let* ((fingertip-center (zero-vector 3))) 
(setf (aref fingertip-center 0) 

(+ (* (aref contact-frame 2) *fingertip-radius*) (aref contact-point 0))) 
(setf (aref fingertip-center 1) 

(+ (* (aref contact-frame 1 2) * fingertip-radius*) (aref contact-point 1))) 
(setf (aref fingertip-center 2) 

(+ (* (aref contact-frame 2 2) *f ingertip-radius*) (aref contact-point 2))) 
fingertip-center) ) 

(defun construct-f ingertip-vector (fingertip-centers) 
(let* ( (n-f ingertips (count-atoms fingertip-centers)) 

(fingertip-vector (zero-vector (* 3 n-f ingertips) )) ) 
(do ((i (+ i 1))) 

( (— i n-f ingertips) fingertip-vector) 
(do (<j <+ j 1))) 
((= j 3)) 
(setf (aref fingertip-vector (+ (* i 3) j) ) (* 2.54 (aref (car fingertip-centers! 
) j)))) 

(setq fingertip-centers (cdr fingertip-centers) ) ) ) ) 

9.5.2 MOVE-TO-GRASP -CENTER auxiliary functions 

(defun construct-grasp-points (force-center force-magnitude) 

(let* ((grasp-points (list (zero-vector 3) (zero-vector 3) (zero-vector 3))) 

(offset-points (offset-wrench-three-contacts force-center force-magnitude) ) ) 
(setf (aref (first grasp-points) 0) (+ (aref (first *contact-f rames*) 3) (aref (fi! 
rst offset-points) 0))) 

(setf (aref (first grasp-points) 1) (+ (aref (first *contact-f rames*) 1 3) (aref (fi! 
rst offset-points) 1))) 

(setf (aref (first grasp-points) 2) (+ (aref (first *contact-f rames*) 2 3) (aref (fi! 
rst offset-points) 2))) 



(setf (aref (second grasp-points) 0) (+ (aref (second *contact-f rames*) 3) (aref (! 
second offset-points) 0))) 

(setf (aref (second grasp-points) 1) (+ (aref (second *contact-f rames*) 1 3) (aref (! 
second offset-points) 1))) 

(setf (aref (second grasp-points) 2) (+ (aref (second *contact-f rames*) 2 3) (aref (! 
second offset-points) 2))) 

(setf (aref (third grasp-points) 0) (+ (aref (third *contact-f rames*) 3) (aref (th! 
ird offset-points) 0))) 

(setf (aref (third grasp-points) 1) (+ (aref (third *contact-f rames*) 1 3) (aref (th! 
ird offset-points) 1) ) ) 

(setf (aref (third grasp-points) 2) (+ (aref (third *contact-f rames*) 2 3) (aref (th! 
ird offset-points) 2) ) ) 
grasp-points) ) 

;;A11 the move functions will return either NIL or an integer. NIL is returned if the 
; /trajectory is not feasible. NIL is also returned when there's transmission 
; /problems in the parallel connection. 

/ 9.7 Move load functions 

/ i.e. generates a trajectory but does not send i! 

t 

/ 9.7.1 Advanced load moves 

(defun move-to-contact-points-load () 

(let* ( (fingertip-vector (construct-f ingertip-vector 

(determine-f ingertip-centers *contact-points* *contact-fram! 
es*))) 

(dur 0.4) ) 
(send tg :move-f ingers-to fingertip-vector :duration dur))) 

(defun move-to-grasp-center-load (force-center force-magnitude) 

(let* ((force-center-hand-frame (transform-points-frame-global *grasp-frame* force-cen! 
ter)) 

(magnitude (* *scale-force* force-magnitude)) 

(grasp-vector-list (construct-grasp-points force-center-hand-frame magnitude) ) 

(fingertip-center-list 

(determine-f ingertip-centers grasp-vector-list *contact-f rames*) ) 
(fingertip-vector (construct-f ingertip-vector fingertip-center-list) ) 
(dur 0.8) ) 
(print-array-list grasp-vector-list) 
(setq *offset-wrench* (list (multiply-array-list 

(list *scale-force* 

(add-array-list (list (first grasp-vector-list) 

(multiply-array-list 
(list -1 (first *contact-points*) ) ) ! 



)))) 



))))) 



)))))) 

(clearscreen) 

(draw-coordinate-system) 

(sphere-3d-grasp-window (first grasp-vector-list) 0.01) 



(multiply-array-list 

(list *scale-force* 

(add-array-list (list (second grasp-vector-list) 

(multiply-array-list 

(list -1 (second *contact-points*) ) ! 

(multiply-array-list 

(list *scale-force* 

(add-array-list (list (third grasp-vector-list) 

(multiply-array-list 

(list -1 (third *contact -points*) )) ! 



(sphere-3d-grasp-window (second grasp-vector-list) 0.01) 
(sphere-3d-grasp-window (third grasp-vector-list) 0.01) 
(sphere-3d-grasp-window force-center-hand-frame 0.02) 
(send tg :move-fingers-to fingertip-vector :duration dur) ) ) 

; 9.7.2 Basic load moves 

(defun move-up-load (Soptional (dist 1.0) &key number-of-segs duration 

(traj-gen *default-tra jectory-gen*) ) 
(let ( (nsegs (if number-of-segs number-of-segs (send traj-gen :default-segs-per-move) ) ! 
) 

(dur (if duration duration (send traj-gen : default-duration) )) ) 
(setf (aref work-transl-vect 0) 0.0 
(aref work-transl-vect 1) 0.0 
(aref work-transl-vect 2) dist) 
(if (send traj-gen :generate-traj (list (list nil nil work-transl-vect) ) 
: number-of-segs nsegs 
: duration dur) 
(progn (setf (aref *object-frame* 2 3) (+ (aref *object-frame* 2 3) (/ dist 2.54! 
))) 

(initialize-global-variables) ) ) ) ) 

(defun move-down-load (fioptional (dist 1.0) &key number-of-segs duration 

(traj-gen *default-tra jectory-gen*) ) 
(let ( (nsegs (if number-of-segs number-of-segs (send traj-gen :default-segs-per-move) ) ! 
) 

(dur (if duration duration (send traj-gen : default-duration) )) ) 
(setf (aref work-transl-vect 0) 0.0 
(aref work-transl-vect 1) 0.0 
(aref work-transl-vect 2) (- dist)) 
(if (send traj-gen :generate-tra j (list (list nil nil work-transl-vect) ) 

: number-of-segs nsegs 
: duration dur) 
(progn (setf (aref *object-f rame* 2 3) (- (aref *object-f rame* 2 3) (/ dist 2.54! 
))) 

(initialize-global-variables) ) ) ) ) 

(defun move-left-load (^optional (dist 1.0) &key number-of-segs duration 

(traj-gen *default -trajectory-gen*) ) 
(let ((nsegs (if number-of-segs number-of-segs (send traj-gen :default-segs-per-move) ) ! 
) 

(dur (if duration duration (send traj-gen : default-duration) )) ) 
(setf (aref work-transl-vect 0) (- dist) 
(aref work-transl-vect 1) 0.0 
(aref work-transl-vect 2) 0.0) 
(if (send traj-gen :generate-traj (list (list nil nil work-transl-vect) ) 

: number-of-segs nsegs 
: duration dur) 
(progn (setf (aref *object-f rame* 3) (- (aref *object-f rame* 3) (/ dist 2.54! 
))) 

(initialize-global-variables) ) ) ) ) 

(defun move-right-load (Soptional (dist 1.0) Skey number-of-segs duration 

(traj-gen *default-tra jectory-gen*) ) 
(let ( (nsegs (if number-of-segs number-of-segs (send traj-gen :default-segs-per-move) ) ! 
) 

(dur (if duration duration (send traj-gen : default-duration) ) ) ) 
(setf (aref work-transl-vect 0) dist 
(aref work-transl-vect 1) 0.0 
(aref work-transl-vect 2) 0.0) 
(if (send traj-gen : gene rate -t raj (list (list nil nil work-transl-vect)) 

: number-of-segs nsegs 
: duration dur) 
(progn (setf (aref *object-f rame* 3) (+ (aref *object-f rame* 3) (/ dist 2.54! 
))) 

(initialize-global-variables) ) ) ) ) 



(defun move-in-load (Soptional (dist 1.0) &key number-of-segs duration 

(traj-gen *default-trajectory-gen*) ) 
(let ( (nsegs (if number-of-segs number-of-segs (send traj-gen :default-segs-per-move) ) ! 
) 

(dur (if duration duration (send traj-gen : default-duration) )) ) 
(setf (aref work-transl-vect 0) 0.0 

(aref work-transl-vect 1) (- dist) 
(aref work-transl-vect 2) 0.0) 
(if (send traj-gen :generate-traj (list (list nil nil work-transl-vect) ) 

: number-of-segs nsegs 
: duration dur) 
(progn (setf (aref *object-frame* 1 3) (- (aref *object-frame* 1 3) (/ dist 2.54! 
))) 

(initialize-global-variables) ) ) ) ) 

(defun move-out-load (^optional (dist 1.0) &key number-of-segs duration 

(traj-gen *default-tra jectory-gen*) ) 
(let ((nsegs (if number-of-segs number-of-segs (send traj-gen :default-segs-per-move) ) ! 
) 

(dur (if duration duration (send traj-gen : default-duration) )) ) 
(setf (aref work-transl-vect 0) 0.0 
(aref work-transl-vect 1) dist 
(aref work-transl-vect 2) 0.0) 
(if (send traj-gen :generate-traj (list (list nil nil work-transl-vect) ) 

: number-of-segs nsegs 
: duration dur) 
(progn (setf (aref *object-f rame* 1 3) (+ (aref *object-f rame* 1 3) (/ dist 2.54! 
))) 

(initialize-global-variables) ) ) ) ) 

(defun rotate-x-load (Soptional (ang 10.0) &key number-of-segs 

duration 

(traj-gen *default-tra jectory-gen*) ) 
;; convert angles from degrees to radians 
(setq ang (* ang 0.01745329)) 

(let ((nsegs (if number-of-segs number-of-segs (send traj-gen :default-segs-per-move) ) ! 
) 

(dur (if duration duration (send traj-gen :default-duration) ) ) 
(temp-frame (make-array '(3 4) : initial-contents '((1 0) 

(0 10 0) 
(0 1 0))))) 
(setf (aref temp-frame 3) (aref *grasp-frame* 3) ) 
(setf (aref temp-frame 1 3) (aref *grasp-frame* 1 3) ) 
(setf (aref temp-frame 2 3) (aref *grasp-frame* 2 3) ) 

(setq *object-frame* (transform-frames-global-frame temp-frame *object-frame*) ) 
(setf (aref temp-frame 1) (cos ang) ) 
(setf (aref temp-frame 2) (* -1 (sin ang))) 
(setf (aref temp-frame 1 1) (sin ang) ) 
(setf (aref temp-frame 1 2) (cos ang)) 

(setq *object-f rame* (transform-frames-frame-global temp-frame *object-f rame*) ) 
(draw-coordinates *object-f rame* 0.5) 

(if (send traj-gen :generate-tra j (list (list 'xhat ang nil)) 
: number-of-segs nsegs : duration dur) 
(progn (setq *object-f rame* (transform-frames-frame-global temp-frame *object-fr! 
ame*) ) 

(initialize-global-variables) ) ) ) ) 

(defun rotate-y-load (Soptional (ang 10.0) &key number-of-segs 

duration 

(traj-gen *default-tra jectory-gen*) ) 
(setq ang (* ang 0.01745329)) 

(let ((nsegs (if number-of-segs number-of-segs (send traj-gen :default-segs-per-move) ) ! 
) 

(dur (if duration duration (send traj-gen : default-duration) ) ) 
(temp-frame (zero-array 3 4))) 



(setf (aref temp-frame 1 1) 

(setf (aref temp-frame 0) 

(setf (aref temp-frame 2) 

(setf (aref temp-frame 2 0) 

(setf (aref temp-frame 2 2) 

(setf (aref temp-frame 3) 

(setf (aref temp-frame 1 3) 

(setf (aref temp-frame 2 3) 



1) 

(cos ang) ) 

(* -1 (sin ang))) 

(sin ang) ) 

(cos ang) ) 

(- (aref *grasp-f rame* 

(- (aref *grasp-frame* 



3) (aref *object-frame* 3))) 

1 3) (aref *object-frame* 13))) 

2 3) (aref *object-frame* 2 3))) 



ame*) ) 



, , . r _ „, (- (aref *grasp-frame* _ „, , 

(if (send traj-gen :generate-traj (list (list 'yhat ang nil)) 

:number-of-segs nsegs : duration dur) 
(progn (setq *object-f rame* (transform-frames-frame-global temp-frame *object-fr! 



(initialize-global-variables) ) ) ) ) 



(defun rotate-z-load (fioptional (ang 10.0) Skey number-of-segs 

duration 

(traj-gen *default-tra jectory-gen*) ) 
(setq ang (* ang 0.01745329)) 

(let ((nsegs (if number-of-segs number-of-segs (send traj-gen :default-segs-per-move) ) ! 
) 

(dur (if duration duration (send traj-gen : default -duration) ) ) 
(temp-frame (zero-array 3 4))) 
(setf (aref temp-frame 2 2) 1) 
(setf (aref temp-frame 0) (cos ang) ) 
(setf (aref temp-frame 1) (* -1 (sin ang) ) ) 
(setf (aref temp-frame 1 0) (sin ang)) 
(setf (aref temp-frame 1 1) (cos ang) ) 

(setf (aref temp-frame 3) (- (aref *grasp-f rame* 3) (aref *object-f rame* 3))) 
(setf (aref temp-frame 1 3) (- (aref *grasp-f rame* 1 3) (aref *object-f rame* 13))) 
(setf (aref temp-frame 2 3) (- (aref *grasp-f rame* 2 3) (aref *object-f rame* 2 3))) 
(if (send traj-gen :generate-traj (list (list ' zhat ang nil)) 

: number-of-segs nsegs : duration dur) 
(progn (setq *object-f rame* (transform-frames-frame-global temp-frame *object-fr! 
ame*) ) 

(initialize-global-variables) ) ) ) ) 

(defun move-finger-load (finger-number displacement-vector) 
(let ((traj-gen *default-tra jectory-gen*) 
(dur 0.3) 

(displacement (zero-vector 9))) 
(setf (aref displacement (+ (* 3 (- finger-number 1)) 0)) (aref displacement-vector 



0)) 
D) 
2)) 



(setf (aref displacement (+ (* 3 (- finger-number 1)) 1)) (aref displacement-vector 
(setf (aref displacement (+ (* 3 (- finger-number 1)) 2)) (aref displacement-vector 
(send traj-gen :move-fingers-by displacement :duration dur))) 



(defun go-hand () 

(send tg :send-traj pc) ) 



9 . 8 Basic moves 



(defun up (&optional (distance 2.0)) 
(if (move-up-load distance) 
(progn (clearscreen) 

(draw-coordinate-system) 
(go-hand) ) ) ) 
(defun down (Soptional (distance 2.0)) 
(if (move-down-load distance) 
(progn (clearscreen) 

(draw-coordinate-system) 
(go-hand) ) ) ) 
(defun right (Soptional (distance 2.0)) 
(if (move-right-load distance) 
(progn (clearscreen) 



(draw-coordinate-system) 
(go-hand) ) ) ) 
(defun left (Soptional (distance 2.0)) 
(if (move-left-load distance) 
(progn (clearscreen) 

(draw-coordinate-system) 
(go-hand) ) ) ) 
(defun in (&optional (distance 2.0)) 
(if (move-in-load distance) 
(progn (clearscreen) 

(draw-coordinate-system) 
(go-hand) ) ) ) 
(defun out (soptional (distance 2.0)) 
(if (move-out-load distance) 
(progn (clearscreen) 

(draw-coordinate-system) 
(go-hand) ) ) ) 
(defun rot+x (Soptional (angle 10.0)) 
(if (rotate-x-load angle) 
(progn (clearscreen) 

(draw-coordinate-system) 
(go-hand) ) ) ) 
(defun rot+y (Soptional (angle 10.0)) 
(if (rotate-y-load angle) 
(progn (clearscreen) 

(draw-coordinate-system) 
(go-hand) ) ) ) 
(defun rot+z (Soptional (angle 10.0)) 
(if (rotate-z-load angle) 
(progn (clearscreen) 

(draw-coordinate-system) 
(go-hand) ) ) ) 
(defun rot-x (Soptional (angle -10.0)) 
(if (rotate-x-load angle) 
(progn (clearscreen) 

(draw-coordinate-system) 
(go-hand) ) ) ) 
(defun rot-y (Soptional (angle -10.0)) 
(if (rotate-y-load angle) 
(progn (clearscreen) 

(draw-coordinate-system) 
(go-hand) ) ) ) 
(defun rot-z (Soptional (angle -10.0)) 
(if (rotate-z-load angle) 
(progn (clearscreen) 

(draw-coordinate-system) 
(go-hand) ) ) ) 

***** 

r 

' 10. Menu 

;; .***********************************^^**^^^ AA ^^^^^^^^^ A ^^^^^^^^^^^ A ^^ AA ^^^^^^^^^^ vtvk , vl:ik . )lt| 
**** 



(defvar menu) 
(defvar moves-menu) 
(defvar dem-menu) 



10.1 Define menu variables 



10.2 Define menu 



(setq menu (tv:make-window 'tv: momentary-menu 

': label '(:string #. (zl: string "Grasp Menu: 
' : geometry (list 2) 



")) 



: borders 3 
: item-list ' ( 



nu) 



-menu) 



"STANDARD GRAPHICS OPTIONS: " :no-select) 

"STANDARD MOTION OPTIONS: " :no-select) 

"Create grasp screen " :funcall make-grasp-screen) 

"Reinit system " :funcall reinit) 

"Clear screen" :funcall clearscreen) 

"Reinit OOLAH trajectory " :funcall init-oolah) 

"Draw coordinate system " :funcall draw-coordinate-system) 

"Reinit VAX trajectory " :funcall init-vax) 

"Change graphics variables" :funcall change-graphics-variables-me ! 

"Go home " :funcall go-home-and-init) 

"" : no-select) 

"BASIC MOVES " :funcall basic -moves -menu) 

"" : no-select) 

"Move finger " :funcall move-finger-menu) 

"" : no-select) 

"" : no-select) 

"SLIP ANALYSIS OPTIONS: " :no-select) 

"ADVANCED HAND ACTUATION OPTIONS: " :no-select) 

"Change global variables " :funcall change-global-variables-menu) 

"JKS move menu " :funcall move-menu) 

"Permissible-twist " :funcall permissible-twist-menu) 

"Move to contact points" :funcall move-to-contact-points) 

"Determine constraint state " :funcall determine-constraint-state! 



"Pick grasp force center " :funcall grasp-force-center-menu) 

"Map constraint space " :funcall map-constraint-state-menu) 

"Controlled slip 

"" : no-select) 

"DEMONSTRATIONS" 

"" : no-select) 

"" : no-select) 

"" : no-select) 

"QUIT" :eval 999)))) 



' :funcall controlled-slip-menu) 
ifuncall demo-menu) 



10.3 Standard graphics options 

10.3.1 MAKE-GRASP-SCREEN 

10.3.2 CLEARSCREEN 

10.3.3 DRAW-COORDINATE-SYSTEM 



(defun draw-coordinate-system () 

(setq view-frame (construct -view-frame angle-x angle-z) ) 
(draw-coordinates *hand-frame* 1) 
(draw-coordinates *object-frame* 0.75) 
(draw-coordinates *grasp-f rame* 0.5) 
(draw-coordinates *contact-f rames* 0.2)) 



10.3.4 CHANGE-GRAPHICS-VARIABLES-MENU 



(defun change-graphics-variables-menu () 
(let ( (zl-user:scale-3d scale-3d) 

(zl-user:x-origin-3d x-origin-3d) 

( zl-user : y-origin-3d y-origin-3d) 

(zl-user: angle-x angle-x) 

(zl-user: angle-z angle-z)) 
( zl-user :choose-user-options zl-user: graphics-variables-menu) 
(setq scale-3d zl-user :scale-3d) 
(setq x-origin-3d zl-user :x-origin-3d) 
(setq y-origin-3d zl-user :y-origin-3d) 
(setq angle-x zl-user: angle-x) 
(setq angle-z zl-user : angle-z) ) ) 



10.4 Standard motion options 
10.4.0 REINIT 



10.4.1 INIT-OOLAH 
(defun init-oolah () 

(send tg :init) ) 

10.4.2 INIT-VAX 
(defun init-vax () 

(send tg :init-traj pc) ) 

10.4.3 GO-HOME-AND-INIT 
(defun go-home-and-init () 

(set -original- values) 

(setq *object-frame* *original-object-frame*) 

(setq *contact -points-object-space* *original-contact-points-object-space*) 

(setq *contact -normals-object-space* *original-contact-normals-object-space*) 

(initialize-global-variables) 

(init-oolah) 

(init-vax) 

(go -home) 

(init-vax) 

(go -home) 

(clearscreen) 

(draw-coordinate-system) ) 

10.4.4 BASIC-MOVES 

(defun basic -moves -menu () 
(do ((i (+ i 1))) 

( (equalp (send moves-menu ': choose) 999) 'Done))) 

(setq moves-menu (tv: make-window ' tv:momentary-menu 

'.•label '(rstring #. (zl: string "Basic moves menu: ••) ) 

' : geometry (list 3) 

' : borders 2 

':item-list '(("OUT" rfuncall out) 

("UP" :funcall up) 

("" : no-select) 

("LEFT" :funcall left) 

("QUIT" :eval 999) 

("RIGHT" rfuncall right) 

("" : no-select) 

("DOWN" :funcall down) 

("IN" rfuncall in) 

("" : no-select) 

("" : no-select) 

("" : no-select) 

("ROT-X " : no-select) 

("ROT-Y " : no-select) 

("ROT-Z " : no-select) 



(" + 
(" + 
(" + 
(" - 
(" - 
(" - 



:funcall rot+x) 

:funcall rot+y) 

rfuncall rot+z) 

rfuncall rot-x) 

rfuncall rot-y) 

rfuncall rot-z) ) ) ) 



10.4.5 MOVE-FINGER-MENU 

(defun move-finger-menu () 

(let ((displacement (make-array '(3)))) 

( zl-user r choose-user-options zl-user r finger-menu) 
(setf (aref displacement 0) zl-user rx-displacement) 
(setf (aref displacement 1) zl-user ry-displacement) 
(setf (aref displacement 2) zl-user rz -displacement) 
(move-finger-load zl-user r finger-number displacement) 
(go-hand) ) ) 

10.5 SLIP-ANALYSIS 



10.5.1 CHANGE-GLOBAL-VARIABLES -MENU 



(defvar variables-menu) 
(defun change-global-variables-menu () 
(do ((i (+ i 1))) 

( (equalp (send variables-menu ': choose) 



999) 'Done))) 



")) 



(setq variables-menu (tv:make-window 'tv: momentary-menu 

': label '(:string #.(zl:string "Variables moves menu: 

' : geometry (list 1) 

' : borders 2 

': item-list '(("Object variables" :funcall object-variable-menu) 

("Contact variables" :funcall contact-variable-menu) 

("Body wrench" :funcall body-wrench-menu) 

("Finger variables" : no-select) 

("" : no-select) 

("QUIT" :eval 999)))) 



(defun object-variable-menu () 

(let ( (zl-user:*object-frame* *object-f rame*) ) 
( zl-user : object-variable-menu) 
(setq *object-f rame* zl-user : *object-f rame*) 
(initialize-global-variables) ) ) 

(defun contact-variable-menu () 

(let ( (zl-user :*contact-points-object-space* *contact-points-object-space*) 

(zl-user :*contact-normals-object-space* *contact-normals-object-space*) ) 
(zl-user : contact-variable-menu) 

(setq *contact-points-object-space* zl-user: *contact -points-object-space*) 
(setq *contact-normals-object-space* zl-user : *contact-normals-ob ject-space*) 
(initialize-global-variables) ) ) 

(defun body-wrench-menu ( ) 

(setq zl-user : *body- wrench* *body-wrench*) 

(zl-user: body-wrench-menu) 

(setq *body-wrench* zl-user : *body-wrench*) ) 

10.5.2 PERMISSIBLE-TWIST-MENU 

(defun permissible-twist-menu () 
(let ((twist (make-array '(6)))) 
(zl-user : permissible-twist-menu) 
(setq twist zl-user :*twist*) 
(permissible-twist twist) ) ) 

10.5.3 DETERMINE -CONSTRAINT-STATE-MENU 

(defun determine-constraint-state-menu () 
(draw-coordinate-system) 
(let ((magnitude 0) 

(force-center (zero-vector 3)) 
(force-center-hand-frame (zero-vector 3) ) ) 
(zl-user : choose-user-options zl-user : constraint-state-menu) 
(setq magnitude zl-user :magnitude) 

(setq force-center (get-mouse-coordinates-grasp-frame) ) 

(setq force-center-hand-frame (transform-points-frame-global *grasp-frame* force-cen! 
ter)) 

(determine-constraint-state force-center-hand-frame magnitude) ) ) 

10.5.4 MAP-CONSTRAINT-STATE-MENU 



(defun map-constraint-state-menu () 



(draw-coordinate-system) 

( zl-user : choose-user-options zl-user : map-state-menu) 
(let ( (x-inc (/ (- zl-user:x-end zl-user :x-start) zl-user : x-steps ) ) 
(y-inc (/ (- zl-user:y-end zl-user:y-start) zl-user:y-steps) ) ) 
(map-constraint-state zl-user :x-start zl-user :x-end x-inc 

zl-user :y-start zl-user :y-end y-inc 
zl-user : magnitude) ) ) 

10.6 ADVANCED-HAND-ACTUATION-OPTIONS 

10.6.1 JKS MOVE -MENU 

10.6.2 MOVE-TO-CONTACT-POINTS 

10.6.3 GRASP -FORCE-CENTER-MENU 

(defun grasp-force-center-menu () 
(let ( (magnitude 0) 

(force-center (zero-vector 3))) 
(zl-user : choose-user-options zl-user : constraint-state-menu) 
(setq magnitude zl-user :magnitude) 

(setq force-center (get-mouse-coordinates-grasp-frame) ) 
(move-to-grasp-center force-center magnitude) ) ) 

10.6.3.1 GRASP-FORCE-CENTER-MENU Auxiliary f unc ! 
tions 

(defun get-mouse-coordinates-grasp-frame () 

(let ( (mouse-coordinates-view-frame (get-mouse-coordinates-view-frame) ) ) 
(transform-normals-view-grasp-frame mouse-coordinates-view-frame) ) ) 

(defun get -mouse-coordinates-view-frame () 

(let ( (mouse-coordinates (get-mouse-coordinates) ) 
(mc-view-frame (zero-vector 3) ) 

(window-height (send grasp-screen :send-pane 'grasp-window 'rheight))) 
(setf (aref mc-view-frame 0) (/ (- (first mouse-coordinates) x-origin-3d) scale-3d) ) 
(setf (aref mc-view-frame 2) (/ (- window-height (+ (second mouse-coordinates) y-ori! 



gin-3d) ) 

mc-view-frame) ) 



scale-3d) ) 



(defun get-mouse-coordinates () 

(let ((mouse-coordinates (cdr (multiple-value-list (tv:with-mouse-and-buttons-grabbed 

(tv:wait-for-mouse-button-down) ) ) ! 
))) 

mouse-coordinates) ) 

(defun transform-normals-view-grasp-frame (point-view-frame) 
(let* ( (grasp-to-view-frame (construct-grasp-to-view-frame) ) 

(all (aref grasp-to-view-frame 0)) 

(al2 (aref grasp-to-view-frame 1) ) 

(a31 (aref grasp-to-view-frame 2 0) ) 

(a32 (aref grasp-to-view-frame 2 1) ) 

(al4 (aref grasp-to-view-frame 3) ) 

(a34 (aref grasp-to-view-frame 2 3) ) 

(determinant (- (* all a32) (* al2 a31) ) ) 

(xv (aref point -view-frame 0) ) 

(zv (aref point -view-frame 2) ) 

(point-grasp-frame (zero-vector 3) ) ) 
(setf (aref point -grasp-frame 0) (/ (+ (* a32 (- xv al4) ) 

(* -1 al2 (- zv a34))) determinant)) 
(setf (aref point-grasp-frame 1) (/ (+ (* -1 a31 (- xv al4) ) 



(* all (- zv a34))) determinant)) 



point -grasp-frame) ) 



(defun construct-grasp-to-view-frame () 

(let ((grasp-to-view-frame (zero-array 3 4))) 



(do ((i (+ i 1))) 
((= i 3)) 
(do <<j (+ j 1))) 
((= j 3)) 
(do (<k (+ k 1))) 
((= k 3)) 
(setf (aref grasp-to-view-frame i j) (+ (* (aref *grasp-frame* k j) 

(aref view-frame k i) ) 
(aref grasp-to-view-frame i j)))))) 
(do (<i (+ i 1))) 
((= i 3)) 
(do ((j (+ j 1))) 
((= j 3)) 
(setf (aref grasp-to-view-frame i 3) (+ (* (aref view-frame j i) 

(- (aref *grasp-f rame* j 3) 



(setf (aref grasp-to-view-frame 2) 0) 

(setf (aref grasp-to-view-frame 10) 0) 

(setf (aref grasp-to-view-frame 1 1) 0) 

(setf (aref grasp-to-view-frame 1 2) 0) 

(setf (aref grasp-to-view-frame 1 3) 0) 

(setf (aref grasp-to-view-frame 2 2) 0) 
grasp-to-view-frame) ) 



(aref view-frame j 3) ) ) 
(aref grasp-to-view-frame i 3) ) ) ) ) 



10.6.4 DEMO-MENU 



(defun demo-menu () 
(do ((i (+ i 1))) 

( (equalp (send dem-menu ': choose) 999) 'Done))) 



10.7 GRASP -MENU 



(defun grasp-menu () 
(do ((i (+ i 1))) 

((equalp (send menu ': choose) 999) 'Done))) 



;;;*************************************************************************************! 
***** 

'' 11. Demos 

/ 

;;;*************************************************************************************! 
***** 



11.1 DEMO -MENU 
(defvar dem-menu) 

(setq dem-menu (tv: make-window 'tv:momentary-menu 

': label '(:string #.(zl: string "Advanced dexterity demonstrations: ")) 

' : geometry (list 1) 

' : borders 2 

': item-list '(("Can demo" rfuncall dlb-can-demo) 

("Can twirl demo" :funcall can-twirl) 

("Can spin demo" :funcall can-spin) 

("Box demo" :funcall box-spin) 

("QUIT" :eval 999)))) 



11.2 Demonstration programs 
11.2.0 DLB-CAN-DEMO 



(defun dlb-can-demo () 

(print "You have 3 seconds to place the can") 



(go-home-and-init ) 

(sleep 3) 

(grab 4.2) 

(sleep 2) 

(move-in-load 5 : duration 0.8) 

(move-out-load 3 : duration 0.8) 



(move-left-load 5 : duration 0.6 
(move-right -load 10 : duration 
(move-left-load 5 : duration 0.6 

(move-up-load 4 : duration 0.6) 
(move-down-load 6 :duration 1.0 
(move-up-load 2 :duration 0.6) 

(rotate-x-load 10 :duration 0.8 
(rotate-x-load -10 rduration 0. 

(rotate-y-load 30 rduration 0.8 
(rotate-y-load -60 rduration 1. 
(rotate-y-load 30 rduration 0.8 



8) 



8) 



6) 



(rotate-z-load 40 rduration 0.8 

(rotate-z-load -80 rduration 1.6) 

(rotate-z-load 40 rduration 0.8 

(go-hand) 

(sleep 15) 

(print "Finished absolute moves") 

(set-cube-frame) 



(move-left-load 5 rduration 0.6) 
(move-right-load 10 rduration 0.8) 
(move-left-load 5 rduration 0.6) 
(move-up-load 4 rduration 0.6) 
(move-down-load 6 rduration 1.0) 
(move-up-load 2 rduration 0.6) 



(rotate-x- 
(rotate-x- 
(rotate-x- 
(rotate-y- 
(rotate-y- 
(rotate-y- 
(rotate-z- 
(rotate-z- 
(rotate-z- 

(sleep 15) 
(go-hand) 



load 20 : 
load -40 
load 20 : 
load 40 : 
load -80 
load 40 : 
load 40 : 
load -80 
load 40 : 



duration 0.8) 
rduration 0.8) 
duration 0.8) 
duration 0.8) 
rduration 1.6) 
duration 0.8) 
duration 0.8) 
rduration 1.6) 
duration 0.8) 



(move-up-load 2 rduration 0.6) 
(move-left-load 2 rduration 0.6) 
(move-down-load 4 rduration 1.2) 
(move-right-load 4 rduration 1.2) 
(move-up-load 4 rduration 1.2) 
(move-left-load 2 rduration 0.6) 
(move-down-load 2 rduration 0.6) 
(sleep 6) 
(go-hand) 
(send tg r back-to-basic) ) 



11.2.1 TWIRL-CAN 



(defun can-twirl () 

(print "You have 3 seconds to place the can") 
(go-home) 



(sleep 3) 

(setq *object-frame* (make-array '(3 4) : initial-contents '((1 0) 



(0 -1 5.5) 
(0 10 -2.7)))) 



(setq *cont act -points-object -space* 

(list (make-array '(3) : initial-contents '(-0.8 1.3 2.5)) 
(make-array '(3) : initial-contents '(0.8 1.3 2.5)) 
(make-array '(3) : initial-contents '(0 -1.3 2.5)))) 
(initialize-global- variables) 
(move-to-contact-points) ) 
(sleep 1) 
(let* ((force-magnitude 1.0) 

(force-center (zero-vector 3))) 
(move-to-grasp-center-load force-center force-magnitude) 
(move-right-load 4 :duration 0.6) 
(move-left-load 8 : duration 1.0) 
(move-right-load 4 :duration 0.6) 
(move-down-load 2 :duration 0.6) 
(move-up-load 4 : duration 1.0) 
(move-down-load 2 :duration 0.6) 
(move-to-grasp-center-load force-center force-magnitude) 

(setq force-magnitude 0.2) 
(setf (aref force-center 0) 0.9) 
(setf (aref force-center 1) 0) 
(move-to-grasp-center-load force-center force-magnitude) 

(setq force-magnitude 1.2) 

(setf (aref force-center 0) 0) 

(move-to-grasp-center-load force-center force-magnitude) 

(move-right-load 4 :duration 0.6) 

(move-left-load 8 : duration 1.0) 

(move-right-load 4 :duration 0.6) 

(move-down-load 2 : duration 0.6) 

(move-up-load 4 : duration 0.8) 

(move-down-load 2 rduration 1.0) 

(move-to-grasp-center-load force-center force-magnitude) 

(go-hand) ) ) 

11.2.2 CAN-CRAWL 

11.2.2 CAN-SPIN 
(defun can-spin () 

(print "You have 2 seconds to place the can") 

(go -home) 

(sleep 2) 

(setq *object-frame* (make-array '(3 4) : initial-contents '((1 0) 



(setq *cont act-points-object -space* 

(list (make-array '(3) : initial-contents '(-0.8 1.3 2.5)) 
(make-array '(3) : initial-contents '(0.8 1.3 2.5)) 
(make-array '(3) : initial-contents '(0 -1.3 2.5)))) 
(initialize-global-variables) 
(move-to-contact -points) 
(sleep 2) 
(let* ((force-magnitude 0.3) 

(force-center (zero-vector 3)) 
(displacement (zero-vector 3) ) ) 
(move-to-grasp-center-load force-center force-magnitude) 

(setq force-magnitude 0.4) 

(setf (aref force-center 0) -0.6) 

(move-to-grasp-center-load force-center force-magnitude) 

(setf (aref displacement 0) 0) 



(0 -1 5.5) 
(0 10 -2.5)))) 



(setf (aref displacement 1) 1) 
(setf (aref displacement 2) 1) 
(move-finger-load 2 displacement) 

(setf (aref displacement 0) 0) 
(setf (aref displacement 1) 2) 
(setf (aref displacement 2) -2) 
(move-finger-load 2 displacement) 

(setf (aref displacement 0) 0) 
(setf (aref displacement 1) -4) 
(setf (aref displacement 2) -1) 
(move-finger-load 2 displacement) 

(setf (aref displacement 0) 1) 
(setf (aref displacement 1) 1) 
(setf (aref displacement 2) 2) 
(move-finger-load 2 displacement) 

(setf (aref displacement 0) -0.5) 
(setf (aref displacement 1) 3) 
(setf (aref displacement 2) -2) 
(move-finger-load 2 displacement) 

(setf (aref displacement 0) -0.5) 
(setf (aref displacement 1) -4.5) 
(setf (aref displacement 2) -1) 
(move-finger-load 2 displacement) 

(setf (aref displacement 0) 0.5) 
(setf (aref displacement 1) 1.5) 
(setf (aref displacement 2) 3) 
(move-finger-load 2 displacement) 

(setf (aref displacement 0) 0) 
(setf (aref displacement 1) 3) 
(setf (aref displacement 2) 1) 
(move-finger-load 2 displacement) 

(setf (aref displacement 0) -0.5) 
(setf (aref displacement 1) 1.5) 
(setf (aref displacement 2) -0.5) 
(move-finger-load 2 displacement) 

(setf (aref displacement 0) 0) 
(setf (aref displacement 1) -4) 
(setf (aref displacement 2) -0.5) 
(move-finger-load 2 displacement) 

(setf (aref displacement 0) 1) 
(setf (aref displacement 1) 1) 
(setf (aref displacement 2) 1) 
(move-finger-load 2 displacement) 

(setf (aref displacement 0) 0) 
(setf (aref displacement 1) 2) 
(setf (aref displacement 2) 1) 
(move-finger-load 2 displacement) 

(setf (aref displacement 0) 0) 
(setf (aref displacement 1) 0.5) 
(setf (aref displacement 2) -2) 
(move-finger-load 2 displacement) 

(setf (aref displacement 0) 0) 
(setf (aref displacement 1) -2.2) 



(setf (aref displacement 2) 0) 
(move-finger-load 2 displacement) 

(setf (aref displacement 0) 0) 
(setf (aref displacement 1) 0) 
(setf (aref displacement 2) 1) 
(move-finger-load 2 displacement) 

(setq force-magnitude 0.5) 

(setf (aref force-center 0) -0.6) 

(move-to-grasp-center-load force-center force-magnitude) 

(setq force-magnitude 0.5) 
(setf (aref force-center 0) 0) 
(move-to-grasp-center-load force-center force-magnitude) 



(go-hand) ) ) 



11.2.3 BOX-SPIN 
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(defun box-spin () 

(print "You have 3 seconds to place the box") 

(go -home) 

(sleep 3) 

(setq *object-f rame* (make-array '(3 4) :init 



(setq *contact-points-object-space* 

(list (make-array ' (3) : initial-contents 

(make-array ' (3) : initial-contents 

(make-array '(3) : initial-contents '(0.8 0.0 -0.8)))) 
(initialize-global-variables) 
(move-to-contact -points-load) 
(move-left-load 7.6) 
(go-hand) 
(clearscreen) 
( draw-coordinate-system) 

(let* ((force-magnitude 0.05) 

(force-center (zero-vector 3) ) 

(displacement (zero-vector 3) ) ) 
(initialize-global-variables) 
(setf (aref force-center 0) 0.8) 

(do <(i (+ i 1))) 
((= i 3)) 
(setq force-magnitude 0.2) 
(setf (aref force-center 0) 0.8) 
(move-to-grasp-center-load force-center force-magnitude) 

Bring the finger back 

(setf (aref displacement 0) 1) 

(setf (aref displacement 1) 0) 

(setf (aref displacement 2) 2) 
(move-finger-load 1 displacement) 

(setf (aref displacement 0) 1.13) 

(setf (aref displacement 1) -1.13) 

(setf (aref displacement 2) 0) 

Push in and rotate 
(move-finger-load 1 displacement) 
(setf (aref displacement 0) 0) 
(setf (aref displacement 1) 0) 
(setf (aref displacement 2) -2.5) 
(move-finger-load 1 displacement) 
(setf (aref displacement 0) -1.13) 



(setf (aref displacement 1) 1.13) 
(setf (aref displacement 2) 0) 
(move-finger-load 1 displacement) 
(setf (aref displacement 0) 1.13) 
(setf (aref displacement 1) 1.13) 
(setf (aref displacement 2) 0) 

Bring the finger back 
(move-finger-load 1 displacement) 
(setf (aref displacement 0) 0) 
(setf (aref displacement 1) 0) 
(setf (aref displacement 2) 2.5) 
(move-finger-load 1 displacement) 
(setf (aref displacement 0) -1.13) 
(setf (aref displacement 1) -1.13) 
(setf (aref displacement 2) 0) 
(move-finger-load 1 displacement) 
(setf (aref displacement 0) 1.13) 
(setf (aref displacement 1) -1.13) 
(setf (aref displacement 2) 0) 

Push in and rotate 
(move-finger-load 1 displacement) 
(setf (aref displacement 0) 0) 
(setf (aref displacement 1) 0) 
(setf (aref displacement 2) -2.5) 
(move-finger-load 1 displacement) 
(setf (aref displacement 0) -1.13) 
(setf (aref displacement 1) 1.13) 
(setf (aref displacement 2) 0) 
(move-finger-load 1 displacement) 
(setf (aref displacement 0) 1.13) 
(setf (aref displacement 1) 1.13) 
(setf (aref displacement 2) 0) 
(move-finger-load 1 displacement) 

Bring the finger back 
(setf (aref displacement 0) 0) 
(setf (aref displacement 1) 0) 
(setf (aref displacement 2) 2.5) 
(move-finger-load 1 displacement) 
(setf (aref displacement 0) -1.13) 
(setf (aref displacement 1) -1.13) 
(setf (aref displacement 2) 0) 
(move-finger-load 1 displacement) 
(setf (aref displacement 0) 1.13) 
(setf (aref displacement 1) -1.13) 
(setf (aref displacement 2) 0) 

Push in and rotate 
(move-finger-load 1 displacement) 
(setf (aref displacement 0) 0) 
(setf (aref displacement 1) 0) 
(setf (aref displacement 2) -2.5) 
(move-finger-load 1 displacement) 
(setf (aref displacement 0) -1.13) 
(setf (aref displacement 1) 1.13) 
(setf (aref displacement 2) 0) 
(move-finger-load 1 displacement) 
(setf (aref displacement 0) 1.13) 
(setf (aref displacement 1) 1.13) 
(setf (aref displacement 2) 0) 

Bring the finger back 
(move-finger-load 1 displacement) 
(setf (aref displacement 0) 0) 



(setf (aref displacement 1) 1) 
(setf (aref displacement 2) 3) 
(move-finger-load 1 displacement) 
(setf (aref displacement 0) -1.13) 
(setf (aref displacement 1) -1.13) 
(setf (aref displacement 2) 0) 
(move-finger-load 1 displacement) 

(setq force-magnitude 0.0) 
(setf (aref force-center 0) 0) 

(move-to-grasp-center-load force-center force-magnitude) 
(move-right-load 7.6) 
(setq force-magnitude 0.3) 
(setf (aref force-center 0) -0.5) 

(move-to-grasp-center-load force-center force-magnitude) 
(move-left-load 7.6)) 
(go-hand) ) ) 



(defun back-rub () 

(print "You have 2 seconds to place the back") 

(go-home) 

(sleep 2) 

(setq *object-frame* (make-array '(3 4) : initial-contents '((1 0) 



(setq *contact -points-object-space* 

(list (make-array '(3) : initial-contents '(-0.8 1.3 2.5)) 
(make-array '(3) : initial-contents '(0.8 1.3 2.5)) 
(make-array '(3) : initial-contents '(0 -1.3 2.5)))) 

(initialize-global-variables) 

(move-to-contact-points) 

(go-hand) ) 



(sleep 2) 

(let* ((force-magnitude 0.3) 

(force-center (zero-vector 3)) 

(displacement (zero-vector 3))) 
(move-to-grasp-center-load force-center force-magnitude) 

(setq force-magnitude 0.4) 

(setf (aref force-center 0) -0.6) 

(move-to-grasp-center-load force-center force-magnitude) 

(setf (aref displacement 0) 0) 
(setf (aref displacement 1) 1) 
(setf (aref displacement 2) 1) 
(move-finger-load 2 displacement) 

(setf (aref displacement 0) 0) 
(setf (aref displacement 1) 2) 
(setf (aref displacement 2) -2) 
(move-finger-load 2 displacement) 

(setf (aref displacement 0) 0) 
(setf (aref displacement 1) -4) 
(setf (aref displacement 2) -1) 
(move-finger-load 2 displacement) 

(setf (aref displacement 0) 1) 
(setf (aref displacement 1) 1) 
(setf (aref displacement 2) 2) 
(move-finger-load 2 displacement) 



(0 -1 5.5) 
(0 10 -2.5)))) 



setf (aref displacement 0) -1) 
setf (aref displacement 1) 3) 
setf (aref displacement 2) -2) 
move-finger-load 2 displacement) 

setf (aref displacement 0) -0.5) 
setf (aref displacement 1) -4.5) 
setf (aref displacement 2) -1) 
move-finger-load 2 displacement) 

setf (aref displacement 0) 0.5) 
setf (aref displacement 1) 1.5) 
setf (aref displacement 2) 3) 
move-finger-load 2 displacement) 

setf (aref displacement 0) 0) 
setf (aref displacement 1) 3) 
setf (aref displacement 2) 1) 
move-finger-load 2 displacement) 

setf (aref displacement 0) -1) 
setf (aref displacement 1) 1.5) 
setf (aref displacement 2) -0.5) 
move-finger-load 2 displacement) 

setf (aref displacement 0) 0) 
setf (aref displacement 1) -5) 
setf (aref displacement 2) -1) 
move-finger-load 2 displacement) 

setf (aref displacement 0) 1) 
setf (aref displacement 1) 1) 
setf (aref displacement 2) 1) 
move-finger-load 2 displacement) 

setf (aref displacement 0) 0) 
setf (aref displacement 1) 2) 
setf (aref displacement 2) 1) 
move-finger-load 2 displacement) 

setf (aref displacement 0) 0) 
setf (aref displacement 1) 0.5) 
setf (aref displacement 2) -2) 
move-finger-load 2 displacement) 

setf (aref displacement 0) 0) 
setf (aref displacement 1) -2.5) 
setf (aref displacement 2) 0) 
move-finger-load 2 displacement) 

setf (aref displacement 0) 0) 
setf (aref displacement 1) 0) 
setf (aref displacement 2) 1) 
move-finger-load 2 displacement) 

setq force-magnitude 0.5) 

setf (aref force-center 0) -0.6) 

move-to-grasp-center-load force-center force-magnitude) 

setq force-magnitude 0.5) 
setf (aref force-center 0) 0) 
move-to-grasp-center-load force-center force-magnitude) 

(go-hand) ) ) 

(defun pic () 
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